From b2ace4a453df5adda7fefcace3e8a80d0ddd64d4 Mon Sep 17 00:00:00 2001 From: TheDevMinerTV Date: Sat, 2 Apr 2022 17:18:56 +0200 Subject: [PATCH] Rework configuration to use LittleFS (#134) Recalibrate your IMUs! --- ci/build.py | 19 +- platformio.ini | 3 + src/GlobalVars.h | 2 + .../CalibrationConfig.cpp} | 56 +-- src/configuration/CalibrationConfig.h | 92 ++++ src/configuration/Configuration.cpp | 305 ++++++++++++ src/configuration/Configuration.h | 65 +++ .../DeviceConfig.h} | 37 +- src/consts.h | 4 +- src/main.cpp | 5 +- src/network/udpclient.h | 3 +- src/network/wifihandler.h | 3 +- src/sensors/bmi160sensor.cpp | 73 ++- src/sensors/bmi160sensor.h | 3 +- src/sensors/icm20948sensor.cpp | 447 +++++------------- src/sensors/icm20948sensor.h | 12 +- src/sensors/mpu6050sensor.cpp | 49 +- src/sensors/mpu6050sensor.h | 4 + src/sensors/mpu9250sensor.cpp | 104 ++-- src/sensors/mpu9250sensor.h | 3 +- src/sensors/sensor.h | 2 +- src/serial/serialcommands.cpp | 10 +- src/utils.h | 24 + test/i2cscan.cpp | 4 +- 24 files changed, 841 insertions(+), 488 deletions(-) rename src/{configuration.cpp => configuration/CalibrationConfig.cpp} (61%) create mode 100644 src/configuration/CalibrationConfig.h create mode 100644 src/configuration/Configuration.cpp create mode 100644 src/configuration/Configuration.h rename src/{configuration.h => configuration/DeviceConfig.h} (63%) diff --git a/ci/build.py b/ci/build.py index 6a880d2..2624c14 100644 --- a/ci/build.py +++ b/ci/build.py @@ -2,6 +2,7 @@ import json import os import shutil from enum import Enum +from textwrap import dedent from typing import List COLOR_ESC = '\033[' @@ -24,11 +25,19 @@ class DeviceConfiguration: self.platformio_board = platformio_board def get_platformio_section(self) -> str: - return f""" -[env:{self.platformio_board}] -platform = {self.platform} -board = {self.platformio_board} -""" + section = dedent(f""" + [env:{self.platformio_board}] + platform = {self.platform} + board = {self.platformio_board}""") + + if self.platform == "espressif32": + section += dedent(""" + lib_deps = + ${env.lib_deps} + lorol/LittleFS_esp32 @ 1.0.6 + """) + + return section def filename(self) -> str: return f"{self.platformio_board}.bin" diff --git a/platformio.ini b/platformio.ini index 9cd51c8..f3e0935 100644 --- a/platformio.ini +++ b/platformio.ini @@ -53,6 +53,9 @@ upload_speed = 921600 ; Uncomment below if you want to build for esp32 ; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards ; [env:esp32] +; lib_deps = +; ${env.lib_deps} +; lorol/LittleFS_esp32 @ 1.0.6 ; platform = espressif32 ; board = esp32dev ; Comment out this line below if you have any trouble uploading the firmware - and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed diff --git a/src/GlobalVars.h b/src/GlobalVars.h index 66a0173..255de08 100644 --- a/src/GlobalVars.h +++ b/src/GlobalVars.h @@ -26,8 +26,10 @@ #include "LEDManager.h" #include "status/StatusManager.h" +#include "configuration/Configuration.h" extern SlimeVR::LEDManager ledManager; extern SlimeVR::Status::StatusManager statusManager; +extern SlimeVR::Configuration::Configuration configuration; #endif diff --git a/src/configuration.cpp b/src/configuration/CalibrationConfig.cpp similarity index 61% rename from src/configuration.cpp rename to src/configuration/CalibrationConfig.cpp index d990f22..75555ad 100644 --- a/src/configuration.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2022 TheDevMinerTV Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -21,43 +21,25 @@ THE SOFTWARE. */ -#include -#include "configuration.h" +#include "CalibrationConfig.h" -DeviceConfig config{}; -bool configLoaded; - -void initializeConfig() { - EEPROM.begin(sizeof(DeviceConfig) + 1); -} - -bool hasConfigStored() { - bool hasConfigStored = false; - EEPROM.get(0, hasConfigStored); - return hasConfigStored; -} - -DeviceConfig *const getConfigPtr() -{ - if (!configLoaded) - { - initializeConfig(); - if (hasConfigStored()) - { - EEPROM.get(1, config); +namespace SlimeVR { + namespace Configuration { + const char* calibrationConfigTypeToString(CalibrationConfigType type) { + switch (type) { + case NONE: + return "NONE"; + case BMI160: + return "BMI160"; + case MPU6050: + return "MPU6050"; + case MPU9250: + return "MPU9250"; + case ICM20948: + return "ICM20948"; + default: + return "UNKNOWN"; + } } - configLoaded = true; } - return &config; } - -void setConfig(const DeviceConfig & newConfig) { - config = newConfig; - saveConfig(); -} - -void saveConfig() { - EEPROM.put(0, true); - EEPROM.put(1, config); - EEPROM.commit(); -} \ No newline at end of file diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h new file mode 100644 index 0000000..9de08a9 --- /dev/null +++ b/src/configuration/CalibrationConfig.h @@ -0,0 +1,92 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H +#define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H + +#include + +namespace SlimeVR { + namespace Configuration { + struct BMI160CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + + // calibration temperature for dynamic compensation + float temperature; + }; + + struct MPU6050CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + }; + + struct MPU9250CalibrationConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + }; + + struct ICM20948CalibrationConfig { + // gyroscope bias + int32_t G[3]; + + // accelerometer bias + int32_t A[3]; + + // compass bias + int32_t C[3]; + }; + + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948 }; + + const char* calibrationConfigTypeToString(CalibrationConfigType type); + + struct CalibrationConfig { + CalibrationConfigType type; + + union { + BMI160CalibrationConfig bmi160; + MPU6050CalibrationConfig mpu6050; + MPU9250CalibrationConfig mpu9250; + ICM20948CalibrationConfig icm20948; + } data; + }; + } +} + +#endif diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp new file mode 100644 index 0000000..d36c641 --- /dev/null +++ b/src/configuration/Configuration.cpp @@ -0,0 +1,305 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifdef ESP32 + // #define CONFIG_LITTLEFS_SPIFFS_COMPAT 1 + // #define CONFIG_LITTLEFS_LFS_YES_TRACE + // #define LFS_YES_TRACE + #include + #define LittleFS LITTLEFS +#else + #include +#endif + +#include "Configuration.h" +#include "consts.h" +#include "utils.h" + +namespace SlimeVR { + namespace Configuration { + CalibrationConfig Configuration::m_EmptyCalibration = {NONE}; + + void Configuration::setup() { + if (m_Loaded) { + return; + } + +#if ESP32 + LittleFS.begin(true); +#else + LittleFS.begin(); +#endif + + if (LittleFS.exists("/config.bin")) { + m_Logger.trace("Found configuration file"); + + File file = LittleFS.open("/config.bin", "r"); + + file.read((uint8_t*)&m_Config.version, sizeof(int32_t)); + + if (m_Config.version < CURRENT_CONFIGURATION_VERSION) { + m_Logger.debug("Configuration is outdated: v%d < v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION); + + if (!runMigrations(m_Config.version)) { + m_Logger.error("Failed to migrate configuration from v%d to v%d", m_Config.version, CURRENT_CONFIGURATION_VERSION); + return; + } + } else { + m_Logger.info("Found up-to-date configuration v%d", m_Config.version); + } + + file.seek(0); + file.read((uint8_t*)&m_Config, sizeof(DeviceConfig)); + file.close(); + } else { + m_Logger.info("No configuration file found, creating new one"); + m_Config.version = CURRENT_CONFIGURATION_VERSION; + save(); + } + + loadCalibrations(); + + m_Loaded = true; + + m_Logger.info("Loaded configuration"); + +#ifdef FULL_DEBUG + print(); +#endif + } + + void Configuration::save() { + for (size_t i = 0; i < m_Calibrations.size(); i++) { + CalibrationConfig config = m_Calibrations[i]; + if (config.type == CalibrationConfigType::NONE) { + continue; + } + + char path[17]; + sprintf(path, "/calibrations/%d", i); + + m_Logger.trace("Saving calibration data for %d", i); + + File file = LittleFS.open(path, "w"); + file.write((uint8_t*)&config, sizeof(CalibrationConfig)); + file.close(); + } + + { + File file = LittleFS.open("/config.bin", "w"); + file.write((uint8_t*)&m_Config, sizeof(DeviceConfig)); + file.close(); + } + + m_Logger.debug("Saved configuration"); + } + + void Configuration::reset() { + LittleFS.format(); + + m_Calibrations.clear(); + m_Config.version = 1; + save(); + + m_Logger.debug("Reset configuration"); + } + + int32_t Configuration::getVersion() const { + return m_Config.version; + } + + size_t Configuration::getCalibrationCount() const { + return m_Calibrations.size(); + } + + CalibrationConfig Configuration::getCalibration(size_t sensorID) const { + if (sensorID >= m_Calibrations.size()) { + return m_EmptyCalibration; + } + + return m_Calibrations.at(sensorID); + } + + void Configuration::setCalibration(size_t sensorID, const CalibrationConfig& config) { + size_t currentCalibrations = m_Calibrations.size(); + + if (sensorID >= currentCalibrations) { + m_Calibrations.resize(sensorID + 1, m_EmptyCalibration); + } + + m_Calibrations[sensorID] = config; + } + + void Configuration::loadCalibrations() { +#ifdef ESP32 + { + File calibrations = LittleFS.open("/calibrations"); + if (!calibrations) { + m_Logger.warn("No calibration data found, creating new directory..."); + + if (!LittleFS.mkdir("/calibrations")) { + m_Logger.error("Failed to create directory: /calibrations"); + return; + } + + calibrations = LittleFS.open("/calibrations"); + } + + if (!calibrations.isDirectory()) { + calibrations.close(); + + m_Logger.warn("Found file instead of directory: /calibrations"); + + if (!LittleFS.remove("/calibrations")) { + m_Logger.error("Failed to remove directory: /calibrations"); + return; + } + + if (!LittleFS.mkdir("/calibrations")) { + m_Logger.error("Failed to create directory: /calibrations"); + return; + } + + calibrations = LittleFS.open("/calibrations"); + } + + m_Logger.debug("Found calibration data directory"); + + while (File f = calibrations.openNextFile()) { + if (f.isDirectory()) { + continue; + } + + m_Logger.trace("Found calibration data file: %s", f.name()); + + CalibrationConfig calibrationConfig; + f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig)); + f.close(); + + uint8_t sensorId = strtoul(calibrations.name(), nullptr, 10); + m_Logger.debug("Found sensor calibration for %s at index %d", calibrationConfigTypeToString(calibrationConfig.type), sensorId); + + setCalibration(sensorId, calibrationConfig); + } + + calibrations.close(); + } +#else + { + if (!LittleFS.exists("/calibrations")) { + m_Logger.warn("No calibration data found, creating new directory..."); + + if (!LittleFS.mkdir("/calibrations")) { + m_Logger.error("Failed to create directory: /calibrations"); + return; + } + + // There's no calibrations here, so we're done + return; + } + + Dir calibrations = LittleFS.openDir("/calibrations"); + while (calibrations.next()) { + File f = calibrations.openFile("r"); + if (!f.isFile()) { + continue; + } + + CalibrationConfig calibrationConfig; + f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig)); + + uint8_t sensorId = strtoul(calibrations.fileName().c_str(), nullptr, 10); + m_Logger.debug("Found sensor calibration for %s at index %d", calibrationConfigTypeToString(calibrationConfig.type), sensorId); + + setCalibration(sensorId, calibrationConfig); + } + } +#endif + } + + bool Configuration::runMigrations(int32_t version) { + return true; + } + + void Configuration::print() { + m_Logger.info("Configuration:"); + m_Logger.info(" Version: %d", m_Config.version); + m_Logger.info(" %d Calibrations:", m_Calibrations.size()); + + for (size_t i = 0; i < m_Calibrations.size(); i++) { + const CalibrationConfig& c = m_Calibrations[i]; + m_Logger.info(" - [%3d] %s", i, calibrationConfigTypeToString(c.type)); + + switch (c.type) { + case CalibrationConfigType::NONE: + break; + + case CalibrationConfigType::BMI160: + m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B)); + + m_Logger.info(" A_Ainv :"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_Ainv[i])); + } + + m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.G_off)); + m_Logger.info(" Temperature: %f", c.data.bmi160.temperature); + + break; + + case CalibrationConfigType::ICM20948: + m_Logger.info(" G: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.G)); + m_Logger.info(" A: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.A)); + m_Logger.info(" C: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.C)); + + break; + + case CalibrationConfigType::MPU9250: + m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B)); + + m_Logger.info(" A_Ainv:"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_Ainv[i])); + } + + m_Logger.info(" M_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_B)); + + m_Logger.info(" M_Ainv:"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.M_Ainv[i])); + } + + m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.G_off)); + + break; + + case CalibrationConfigType::MPU6050: + m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)); + m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)); + + break; + } + } + } + } +} diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h new file mode 100644 index 0000000..2bdeb74 --- /dev/null +++ b/src/configuration/Configuration.h @@ -0,0 +1,65 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SLIMEVR_CONFIGURATION_CONFIGURATION_H +#define SLIMEVR_CONFIGURATION_CONFIGURATION_H + +#include + +#include "DeviceConfig.h" +#include "logging/Logger.h" + +namespace SlimeVR { + namespace Configuration { + class Configuration { + public: + void setup(); + + void save(); + void reset(); + + void print(); + + int32_t getVersion() const; + + size_t getCalibrationCount() const; + CalibrationConfig getCalibration(size_t sensorID) const; + void setCalibration(size_t sensorID, const CalibrationConfig& config); + + private: + void loadCalibrations(); + bool runMigrations(int32_t version); + + bool m_Loaded = false; + + DeviceConfig m_Config{}; + std::vector m_Calibrations; + + Logging::Logger m_Logger = Logging::Logger("Configuration"); + + static CalibrationConfig m_EmptyCalibration; + }; + } +} + +#endif diff --git a/src/configuration.h b/src/configuration/DeviceConfig.h similarity index 63% rename from src/configuration.h rename to src/configuration/DeviceConfig.h index 529583a..b708ca7 100644 --- a/src/configuration.h +++ b/src/configuration/DeviceConfig.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2022 TheDevMinerTV Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -21,30 +21,17 @@ THE SOFTWARE. */ -#ifndef SLIMEVR_CONFIG_H_ -#define SLIMEVR_CONFIG_H_ +#ifndef SLIMEVR_CONFIGURATION_DEVICECONFIG_H +#define SLIMEVR_CONFIGURATION_DEVICECONFIG_H -struct CalibrationConfig { - //accel offsets and correction matrix - float A_B[3]; - float A_Ainv[3][3]; - // mag offsets and correction matrix - float M_B[3]; - float M_Ainv[3][3]; - //raw offsets, determined for gyro at rest - float G_off[3]; - // calibration temperature for dynamic compensation - float temperature; -}; +#include "CalibrationConfig.h" -struct DeviceConfig { - CalibrationConfig calibration[2]; - int deviceId; - int deviceMode; -}; +namespace SlimeVR { + namespace Configuration { + struct DeviceConfig { + int32_t version; + }; + } +} -DeviceConfig * const getConfigPtr(); -void setConfig(const DeviceConfig & config); -void saveConfig(); - -#endif // SLIMEVR_CONFIG_H_ \ No newline at end of file +#endif diff --git a/src/consts.h b/src/consts.h index 83dfabf..5ebf4da 100644 --- a/src/consts.h +++ b/src/consts.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2021 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -72,4 +72,6 @@ #define HARDWARE_MCU 0 #endif +#define CURRENT_CONFIGURATION_VERSION 1 + #endif // SLIMEVR_CONSTS_H_ diff --git a/src/main.cpp b/src/main.cpp index 2fe8a79..44e26bb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,7 +24,7 @@ #include "Wire.h" #include "ota.h" #include "sensors/SensorManager.h" -#include "configuration.h" +#include "configuration/Configuration.h" #include "network/network.h" #include "globals.h" #include "credentials.h" @@ -39,6 +39,7 @@ SlimeVR::Logging::Logger logger("SlimeVR"); SlimeVR::Sensors::SensorManager sensorManager; SlimeVR::LEDManager ledManager(LED_PIN); SlimeVR::Status::StatusManager statusManager; +SlimeVR::Configuration::Configuration configuration; int sensorToCalibrate = -1; bool blinking = false; @@ -61,6 +62,7 @@ void setup() statusManager.setStatus(SlimeVR::Status::LOADING, true); ledManager.setup(); + configuration.setup(); SerialCommands::setUp(); @@ -75,7 +77,6 @@ void setup() #endif Wire.setClock(I2C_SPEED); - getConfigPtr(); // Wait for IMU to boot delay(500); diff --git a/src/network/udpclient.h b/src/network/udpclient.h index dece941..b569787 100644 --- a/src/network/udpclient.h +++ b/src/network/udpclient.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2021 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -26,7 +26,6 @@ #include #include #include "quat.h" -#include "configuration.h" #include "sensors/sensor.h" #include "wifihandler.h" #include "globals.h" diff --git a/src/network/wifihandler.h b/src/network/wifihandler.h index e24213f..0c2cd4c 100644 --- a/src/network/wifihandler.h +++ b/src/network/wifihandler.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2021 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -23,7 +23,6 @@ #ifndef SLIMEVR_WIFI_H_ #define SLIMEVR_WIFI_H_ -#include "configuration.h" #ifdef ESP8266 #include #else diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 63a595a..86d9574 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 S.J. Remington, SlimeVR contributors + Copyright (c) 2021 S.J. Remington & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -85,8 +85,26 @@ void BMI160Sensor::motionSetup() { ledManager.off(); } - DeviceConfig * const config = getConfigPtr(); - calibration = &config->calibration[sensorId]; + // Initialize the configuration + { + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::BMI160: + m_Calibration = sensorCalibration.data.bmi160; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + } + working = true; } @@ -148,7 +166,7 @@ void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3]) #endif float temperature = getTemperature(); - float tempDiff = temperature - calibration->temperature; + float tempDiff = temperature - m_Calibration.temperature; uint8_t quant = map(temperature, 15, 75, 0, 12); int16_t ax, ay, az; @@ -158,9 +176,9 @@ void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3]) // TODO: Sensitivity over temp compensation? // TODO: Cross-axis sensitivity compensation? - Gxyz[0] = ((float)gx - (calibration->G_off[0] + (tempDiff * LSB_COMP_PER_TEMP_X_MAP[quant]))) * GSCALE; - Gxyz[1] = ((float)gy - (calibration->G_off[1] + (tempDiff * LSB_COMP_PER_TEMP_Y_MAP[quant]))) * GSCALE; - Gxyz[2] = ((float)gz - (calibration->G_off[2] + (tempDiff * LSB_COMP_PER_TEMP_Z_MAP[quant]))) * GSCALE; + Gxyz[0] = ((float)gx - (m_Calibration.G_off[0] + (tempDiff * LSB_COMP_PER_TEMP_X_MAP[quant]))) * GSCALE; + Gxyz[1] = ((float)gy - (m_Calibration.G_off[1] + (tempDiff * LSB_COMP_PER_TEMP_Y_MAP[quant]))) * GSCALE; + Gxyz[2] = ((float)gz - (m_Calibration.G_off[2] + (tempDiff * LSB_COMP_PER_TEMP_Z_MAP[quant]))) * GSCALE; Axyz[0] = (float)ax; Axyz[1] = (float)ay; @@ -169,10 +187,10 @@ void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3]) #if useFullCalibrationMatrix == true float temp[3]; for (uint8_t i = 0; i < 3; i++) - temp[i] = (Axyz[i] - calibration->A_B[i]); - Axyz[0] = calibration->A_Ainv[0][0] * temp[0] + calibration->A_Ainv[0][1] * temp[1] + calibration->A_Ainv[0][2] * temp[2]; - Axyz[1] = calibration->A_Ainv[1][0] * temp[0] + calibration->A_Ainv[1][1] * temp[1] + calibration->A_Ainv[1][2] * temp[2]; - Axyz[2] = calibration->A_Ainv[2][0] * temp[0] + calibration->A_Ainv[2][1] * temp[1] + calibration->A_Ainv[2][2] * temp[2]; + temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + Axyz[0] = m_Calibration.A_Ainv[0][0] * temp[0] + m_Calibration.A_Ainv[0][1] * temp[1] + m_Calibration.A_Ainv[0][2] * temp[2]; + Axyz[1] = m_Calibration.A_Ainv[1][0] * temp[0] + m_Calibration.A_Ainv[1][1] * temp[1] + m_Calibration.A_Ainv[1][2] * temp[2]; + Axyz[2] = m_Calibration.A_Ainv[2][0] * temp[0] + m_Calibration.A_Ainv[2][1] * temp[1] + m_Calibration.A_Ainv[2][2] * temp[2]; #else for (uint8_t i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - calibration->A_B[i]); @@ -183,13 +201,12 @@ void BMI160Sensor::startCalibration(int calibrationType) { ledManager.on(); m_Logger.debug("Gathering raw data for device calibration..."); - DeviceConfig * const config = getConfigPtr(); // Wait for sensor to calm down before calibration m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); delay(2000); float temperature = getTemperature(); - config->calibration[sensorId].temperature = temperature; + m_Calibration.temperature = temperature; uint16_t gyroCalibrationSamples = 2500; float rawGxyz[3] = {0}; @@ -209,12 +226,12 @@ void BMI160Sensor::startCalibration(int calibrationType) { ledManager.off(); } - config->calibration[sensorId].G_off[0] = rawGxyz[0] / gyroCalibrationSamples; - config->calibration[sensorId].G_off[1] = rawGxyz[1] / gyroCalibrationSamples; - config->calibration[sensorId].G_off[2] = rawGxyz[2] / gyroCalibrationSamples; + m_Calibration.G_off[0] = rawGxyz[0] / gyroCalibrationSamples; + m_Calibration.G_off[1] = rawGxyz[1] / gyroCalibrationSamples; + m_Calibration.G_off[2] = rawGxyz[2] / gyroCalibrationSamples; #ifdef DEBUG_SENSOR - m_Logger.trace("Gyro calibration results: %f %f %f", config->calibration[sensorId].G_off[0], config->calibration[sensorId].G_off[1], config->calibration[sensorId].G_off[2]); + m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Calibration[sensorId].G_off)); #endif // Blink calibrating led before user should rotate the sensor @@ -251,16 +268,24 @@ void BMI160Sensor::startCalibration(int calibrationType) { m_Logger.debug("{"); for (int i = 0; i < 3; i++) { - config->calibration[sensorId].A_B[i] = A_BAinv[0][i]; - config->calibration[sensorId].A_Ainv[0][i] = A_BAinv[1][i]; - config->calibration[sensorId].A_Ainv[1][i] = A_BAinv[2][i]; - config->calibration[sensorId].A_Ainv[2][i] = A_BAinv[3][i]; + m_Calibration.A_B[i] = A_BAinv[0][i]; + m_Calibration.A_Ainv[0][i] = A_BAinv[1][i]; + m_Calibration.A_Ainv[1][i] = A_BAinv[2][i]; + m_Calibration.A_Ainv[2][i] = A_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); } m_Logger.debug("}"); - m_Logger.debug("Now Saving EEPROM"); - setConfig(*config); - m_Logger.debug("Finished Saving EEPROM"); + + m_Logger.debug("Saving the calibration data"); + + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::BMI160; + calibration.data.bmi160 = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + + m_Logger.debug("Saved the calibration data"); + m_Logger.info("Calibration data gathered"); delay(5000); } diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 56ef671..1bc198e 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -41,11 +41,12 @@ class BMI160Sensor : public Sensor { float getTemperature(); private: BMI160 imu {}; - CalibrationConfig * calibration; float q[4] {1.0f, 0.0f, 0.0f, 0.0f}; // Loop timing globals uint32_t now = 0, last = 0; //micros() timers float deltat = 0; //loop time in seconds + + SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration; }; #endif diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 69f7adf..680c376 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 S.J. Remington, SlimeVR contributors + Copyright (c) 2021 S.J. Remington & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -23,7 +23,6 @@ #include "icm20948sensor.h" #include "calibration.h" #include -#include // for 8266, save the current bias values to eeprom #include "network/network.h" #include "GlobalVars.h" @@ -34,324 +33,122 @@ int bias_save_periods[] = { 120, 180, 300, 600, 600 }; // 2min + 3min + 5min + 1 // #define ENABLE_TAP false // #endif -void ICM20948Sensor::save_bias(bool repeat) { - #if defined(SAVE_BIAS) && SAVE_BIAS -#ifdef DEBUG_SENSOR - m_Logger.trace("Saving Bias"); -#endif - #if ESP8266 - int8_t count; - int32_t bias_a[3], bias_g[3], bias_m[3]; - - imu.GetBiasGyroX(&bias_g[0]); - imu.GetBiasGyroY(&bias_g[1]); - imu.GetBiasGyroZ(&bias_g[2]); - - imu.GetBiasAccelX(&bias_a[0]); - imu.GetBiasAccelY(&bias_a[1]); - imu.GetBiasAccelZ(&bias_a[2]); - - #if !USE_6_AXIS - imu.GetBiasCPassX(&bias_m[0]); - imu.GetBiasCPassY(&bias_m[1]); - imu.GetBiasCPassZ(&bias_m[2]); - #endif - - bool gyro_set = bias_g[0] && bias_g[1] && bias_g[2]; - bool accel_set = bias_a[0] && bias_a[1] && bias_a[2]; - #if !USE_6_AXIS - bool CPass_set = bias_m[0] && bias_m[1] && bias_m[2]; - #endif - - EEPROM.begin(4096); // max memory usage = 4096 - EEPROM.get(addr + 100, count); // 1st imu counter in EEPROM addr: 0x69+100=205, 2nd addr: 0x68+100=204 - -#ifdef DEBUG_SENSOR - m_Logger.trace("[0x%02X] EEPROM position: %d, count: %d", addr, addr + 100, count); -#endif - - if(count < 0 || count > 42) { - count = sensorId; // 1st imu counter is even number, 2nd is odd - } else if(repeat) { - count++; - } - EEPROM.put(addr + 100, count); - -#ifdef DEBUG_SENSOR - m_Logger.trace("[0x%02X] bias gyro save(%d): [%d, %d, %d]", addr, count * 12, bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("[0x%02X] bias accel save(%d): [%d, %d, %d]", addr, count * 12, bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("[0x%02X] bias CPass save(%d): [%d, %d, %d]", addr, count * 12, bias_m[0], bias_m[1], bias_m[2]); -#endif - - if (gyro_set) { - EEPROM.put(1024 + (count * 12), bias_g); // 1024 ~ 2008 - } - if (accel_set) { - EEPROM.put(2046 + (count * 12), bias_a); // 2046 ~ 3030 - } - #if !USE_6_AXIS - if (CPass_set) { - EEPROM.put(3072 + (count * 12), bias_m); // 3072 ~ 4056 - } - #endif - EEPROM.end(); // save and end - if (repeat) { - bias_save_counter++; - // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. - if (sizeof(bias_save_periods) != bias_save_counter) { - timer.in(bias_save_periods[bias_save_counter] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->save_bias(true); return false; }, this); - } - } - #elif ESP32 - int bias_a[3], bias_g[3], bias_m[3]; - - imu.GetBiasGyroX(&bias_g[0]); - imu.GetBiasGyroY(&bias_g[1]); - imu.GetBiasGyroZ(&bias_g[2]); - - imu.GetBiasAccelX(&bias_a[0]); - imu.GetBiasAccelY(&bias_a[1]); - imu.GetBiasAccelZ(&bias_a[2]); - - imu.GetBiasCPassX(&bias_m[0]); - imu.GetBiasCPassY(&bias_m[1]); - imu.GetBiasCPassZ(&bias_m[2]); - - bool accel_set = bias_a[0] && bias_a[1] && bias_a[2]; - bool gyro_set = bias_g[0] && bias_g[1] && bias_g[2]; - bool mag_set = bias_m[0] && bias_m[1] && bias_m[2]; - -#ifdef DEBUG_SENSOR - m_Logger.trace("bias gyro on IMU: %d, %d, %d", bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("bias accel on IMU: %d, %d, %d", bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("bias mag on IMU: %d, %d, %d", bias_m[0], bias_m[1], bias_m[2]); -#endif - - bool auxiliary = sensorId == 1; - if (accel_set) { - // Save accel - prefs.putInt(auxiliary ? "ba01" : "ba00", bias_a[0]); - prefs.putInt(auxiliary ? "ba11" : "ba10", bias_a[1]); - prefs.putInt(auxiliary ? "ba21" : "ba20", bias_a[2]); - -#ifdef DEBUG_SENSOR - m_Logger.trace("Wrote Accel Bias"); -#endif - } - - if (gyro_set) { - // Save gyro - prefs.putInt(auxiliary ? "bg01" : "bg00", bias_g[0]); - prefs.putInt(auxiliary ? "bg11" : "bg10", bias_g[1]); - prefs.putInt(auxiliary ? "bg21" : "bg20", bias_g[2]); - -#ifdef DEBUG_SENSOR - m_Logger.trace("Wrote Gyro Bias"); -#endif - } - - if (mag_set) { - // Save mag - prefs.putInt(auxiliary ? "bm01" : "bm00", bias_m[0]); - prefs.putInt(auxiliary ? "bm11" : "bm10", bias_m[1]); - prefs.putInt(auxiliary ? "bm21" : "bm20", bias_m[2]); - -#ifdef DEBUG_SENSOR - m_Logger.trace("Wrote Mag Bias"); -#endif - } - #endif - - if (repeat) { - bias_save_counter++; - // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. - if (sizeof(bias_save_periods) != bias_save_counter) - { - timer.in(bias_save_periods[bias_save_counter] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->save_bias(true); return false; }, this); - } - } - +void ICM20948Sensor::save_bias(bool repeat) { +#if defined(SAVE_BIAS) && SAVE_BIAS + #ifdef DEBUG_SENSOR + m_Logger.trace("Saving Bias"); #endif + + imu.GetBiasGyroX(&m_Calibration.G[0]); + imu.GetBiasGyroY(&m_Calibration.G[1]); + imu.GetBiasGyroZ(&m_Calibration.G[2]); + + imu.GetBiasAccelX(&m_Calibration.A[0]); + imu.GetBiasAccelY(&m_Calibration.A[1]); + imu.GetBiasAccelZ(&m_Calibration.A[2]); + + #if !USE_6_AXIS + imu.GetBiasCPassX(&m_Calibration.C[0]); + imu.GetBiasCPassY(&m_Calibration.C[1]); + imu.GetBiasCPassZ(&m_Calibration.C[2]); + #endif + + #ifdef DEBUG_SENSOR + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); + #if !USE_6_AXIS + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); + #endif + #endif + + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::ICM20948; + calibration.data.icm20948 = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + + if (repeat) { + bias_save_counter++; + // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. + if (sizeof(bias_save_periods) != bias_save_counter) { + timer.in( + bias_save_periods[bias_save_counter] * 1000, + [](void* arg) -> bool { + ((ICM20948Sensor*)arg)->save_bias(true); + return false; + }, + this); + } + } +#endif } void ICM20948Sensor::load_bias() { - #if defined(LOAD_BIAS) && LOAD_BIAS - #if ESP8266 - int8_t count; - int32_t bias_g[3], bias_a[3], bias_m[3]; - count = 0; - EEPROM.begin(4096); // max memory usage = 4096 - EEPROM.get(addr + 100, count); // 1st imu counter in EEPROM addr: 0x69+100=205, 2nd addr: 0x68+100=204 - #ifdef DEBUG_SENSOR - m_Logger.trace("[0x%02X] EEPROM position: %d, count: %d", addr, addr + 100, count); + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); + #if !USE_6_AXIS + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); + #endif #endif - if(count < 0 || count > 42) { - count = sensorId; // 1st imu counter is even number, 2nd is odd - EEPROM.put(addr + 100, count); - } +#if defined(LOAD_BIAS) && LOAD_BIAS + imu.SetBiasGyroX(m_Calibration.G[0]); + imu.SetBiasGyroY(m_Calibration.G[1]); + imu.SetBiasGyroZ(m_Calibration.G[2]); - EEPROM.get(1024 + (count * 12), bias_g); // 1024 ~ 2008 - EEPROM.get(2046 + (count * 12), bias_a); // 2046 ~ 3030 - EEPROM.get(3072 + (count * 12), bias_m); // 3072 ~ 4056 - EEPROM.end(); + imu.SetBiasAccelX(m_Calibration.A[0]); + imu.SetBiasAccelY(m_Calibration.A[1]); + imu.SetBiasAccelZ(m_Calibration.A[2]); -#ifdef DEBUG_SENSOR - m_Logger.trace("[0x%02X] EEPROM gyro get(%d): [%d, %d, %d]", addr, count * 12, bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("[0x%02X] EEPROM accel get(%d): [%d, %d, %d]", addr, count * 12, bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("[0x%02X] EEPROM CPass get(%d): [%d, %d, %d]", addr, count * 12, bias_m[0], bias_m[1], bias_m[2]); + #if !USE_6_AXIS + imu.SetBiasCPassX(m_Calibration.C[0]); + imu.SetBiasCPassY(m_Calibration.C[1]); + imu.SetBiasCPassZ(m_Calibration.C[2]); + #endif +#else + #if BIAS_DEBUG + // Sets all bias to 90 + imu.SetBiasGyroX(90); + imu.SetBiasGyroY(90); + imu.SetBiasGyroZ(90); + + imu.SetBiasAccelX(90); + imu.SetBiasAccelY(90); + imu.SetBiasAccelZ(90); + + imu.SetBiasCPassX(90); + imu.SetBiasCPassY(90); + imu.SetBiasCPassZ(90); + + int bias_gyro[3], bias_accel[3], bias_compass[3]; + + // Reloads all bias from memory + imu.GetBiasGyroX(&bias_gyro[0]); + imu.GetBiasGyroY(&bias_gyro[1]); + imu.GetBiasGyroZ(&bias_gyro[2]); + + imu.GetBiasAccelX(&bias_accel[0]); + imu.GetBiasAccelY(&bias_accel[1]); + imu.GetBiasAccelZ(&bias_accel[2]); + + imu.GetBiasCPassX(&bias_compass[0]); + imu.GetBiasCPassY(&bias_compass[1]); + imu.GetBiasCPassZ(&bias_compass[2]); + + #ifdef FULL_DEBUG + m_Logger.trace("All set bias should be 90"); + + m_Logger.trace("Gyrometer bias : [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_gyro)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_accel)); + m_Logger.trace("Compass bias : [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_compass)); + #endif + #endif #endif - - imu.SetBiasGyroX(bias_g[0]); - imu.SetBiasGyroY(bias_g[1]); - imu.SetBiasGyroZ(bias_g[2]); - - imu.SetBiasAccelX(bias_a[0]); - imu.SetBiasAccelY(bias_a[1]); - imu.SetBiasAccelZ(bias_a[2]); - - imu.SetBiasCPassX(bias_m[0]); - imu.SetBiasCPassY(bias_m[1]); - imu.SetBiasCPassZ(bias_m[2]); - - #elif ESP32 - bool auxiliary = sensorId == 1; - - int32_t bias_a[3], bias_g[3], bias_m[3]; - - bias_a[0] = prefs.getInt(auxiliary ? "ba01" : "ba00", 0); - bias_a[1] = prefs.getInt(auxiliary ? "ba11" : "ba10", 0); - bias_a[2] = prefs.getInt(auxiliary ? "ba21" : "ba20", 0); - - bias_g[0] = prefs.getInt(auxiliary ? "bg01" : "bg00", 0); - bias_g[1] = prefs.getInt(auxiliary ? "bg11" : "bg10", 0); - bias_g[2] = prefs.getInt(auxiliary ? "bg21" : "bg20", 0); - - bias_m[0] = prefs.getInt(auxiliary ? "bm01" : "bm00", 0); - bias_m[1] = prefs.getInt(auxiliary ? "bm11" : "bm10", 0); - bias_m[2] = prefs.getInt(auxiliary ? "bm21" : "bm20", 0); - - imu.SetBiasGyroX(bias_g[0]); - imu.SetBiasGyroY(bias_g[1]); - imu.SetBiasGyroZ(bias_g[2]); - - imu.SetBiasAccelX(bias_a[0]); - imu.SetBiasAccelY(bias_a[1]); - imu.SetBiasAccelZ(bias_a[2]); - - imu.SetBiasCPassX(bias_m[0]); - imu.SetBiasCPassY(bias_m[1]); - imu.SetBiasCPassZ(bias_m[2]); - - // Display both values from ESP32 and read back from IMU -#ifdef DEBUG_SENSOR - m_Logger.trace("On IMU and on ESP32 should match"); - m_Logger.trace("bias gyro on ESP32 : [%d, %d, %d]", bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("bias accel on ESP32: [%d, %d, %d]", bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("bias mag on ESP32 : [%d, %d, %d]", bias_m[0], bias_m[1], bias_m[2]); - - imu.GetBiasGyroX(&bias_g[0]); - imu.GetBiasGyroY(&bias_g[1]); - imu.GetBiasGyroZ(&bias_g[2]); - - imu.GetBiasAccelX(&bias_a[0]); - imu.GetBiasAccelY(&bias_a[1]); - imu.GetBiasAccelZ(&bias_a[2]); - - imu.GetBiasCPassX(&bias_m[0]); - imu.GetBiasCPassY(&bias_m[1]); - imu.GetBiasCPassZ(&bias_m[2]); - - m_Logger.trace("bias gyro on IMU : [%d, %d, %d]", bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("bias accel on IMU: [%d, %d, %d]", bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("bias mag on IMU : [%d, %d, %d]", bias_m[0], bias_m[1], bias_m[2]); -#endif - - #endif // #if esp8266 / elif ESP32 - - #if BIAS_DEBUG - int bias_a[3], bias_g[3], bias_m[3]; - - imu.GetBiasGyroX(&bias_g[0]); - imu.GetBiasGyroY(&bias_g[1]); - imu.GetBiasGyroZ(&bias_g[2]); - - imu.GetBiasAccelX(&bias_a[0]); - imu.GetBiasAccelY(&bias_a[1]); - imu.GetBiasAccelZ(&bias_a[2]); - - imu.GetBiasCPassX(&bias_m[0]); - imu.GetBiasCPassY(&bias_m[1]); - imu.GetBiasCPassZ(&bias_m[2]); - - m_Logger.trace("Starting Gyro Bias is %d, %d, %d", bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("Starting Accel Bias is %d, %d, %d", bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("Starting CPass Bias is %d, %d, %d", bias_m[0], bias_m[1], bias_m[2]); - - //Sets all bias to 90 - bias_g[0] = 90; - bias_g[1] = 90; - bias_g[2] = 90; - bias_a[0] = 90; - bias_a[1] = 90; - bias_a[2] = 90; - bias_m[0] = 90; - bias_m[1] = 90; - bias_m[2] = 90; - - //Sets all bias to 0 in memory - imu.SetBiasGyroX(bias_g[0]); - imu.SetBiasGyroY(bias_g[1]); - imu.SetBiasGyroZ(bias_g[2]); - - imu.SetBiasAccelX(bias_a[0]); - imu.SetBiasAccelY(bias_a[1]); - imu.SetBiasAccelZ(bias_a[2]); - - imu.SetBiasCPassX(bias_m[0]); - imu.SetBiasCPassY(bias_m[1]); - imu.SetBiasCPassZ(bias_m[2]); - - //Sets all bias to 0 - bias_g[0] = 0; - bias_g[1] = 0; - bias_g[2] = 0; - bias_a[0] = 0; - bias_a[1] = 0; - bias_a[2] = 0; - bias_m[0] = 0; - bias_m[1] = 0; - bias_m[2] = 0; - - //Reloads all bias from memory - imu.GetBiasGyroX(&bias_g[0]); - imu.GetBiasGyroY(&bias_g[1]); - imu.GetBiasGyroZ(&bias_g[2]); - - imu.GetBiasAccelX(&bias_a[0]); - imu.GetBiasAccelY(&bias_a[1]); - imu.GetBiasAccelZ(&bias_a[2]); - - imu.GetBiasCPassX(&bias_m[0]); - imu.GetBiasCPassY(&bias_m[1]); - imu.GetBiasCPassZ(&bias_m[2]); - - m_Logger.trace("All set bias should be 90"); - - m_Logger.trace("Set Gyro Bias is %d, %d, %d", bias_g[0], bias_g[1], bias_g[2]); - m_Logger.trace("Set Accel Bias is %d, %d, %d", bias_a[0], bias_a[1], bias_a[2]); - m_Logger.trace("Set CPass Bias is %d, %d, %d", bias_m[0], bias_m[1], bias_m[2]); - #endif // BIAS_DEBUG - #endif // LOAD_BIAS } void ICM20948Sensor::motionSetup() { - #ifdef ESP32 - prefs.begin("ICM20948", false); - #endif - - #ifdef DEBUG_SENSOR + #ifdef FULL_DEBUG imu.enableDebugging(Serial); #endif // SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error @@ -482,8 +279,30 @@ void ICM20948Sensor::motionSetup() { m_Logger.fatal("Failed to reset FIFO"); return; } - - load_bias(); + +#if LOAD_BIAS + // Initialize the configuration + { + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::ICM20948: + m_Calibration = sensorCalibration.data.icm20948; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + } +#endif + + load_bias(); lastData = millis(); working = true; @@ -614,15 +433,7 @@ void ICM20948Sensor::sendData() { void ICM20948Sensor::startCalibration(int calibrationType) { // 20948 does continuous calibration - // If ESP32, manually force a new save - #ifdef ESP32 - save_bias(false); - #endif - // If 8266, save the current bias values to eeprom - #ifdef ESP8266 - // Types are int, device config saves float - need to save and load like mpu6050 does - save_bias(false); - #endif + save_bias(false); } //You need to override the library's initializeDMP to change some settings diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 0c88abf..0e55d11 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -26,9 +26,6 @@ #include #include "sensor.h" #include // Used for periodically saving bias -#ifdef ESP32 -#include // ICM bias saving. ESP8266 use eprom -#endif class ICM20948Sensor : public Sensor { @@ -51,14 +48,11 @@ private: ICM_20948_Device_t pdev; icm_20948_DMP_data_t dmpData{}; + SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration; + #define OVERRIDEDMPSETUP true -#ifdef ESP32 - Preferences prefs; + Timer<> timer = timer_create_default(); -#endif -#ifdef ESP8266 - Timer<> timer = timer_create_default(); -#endif // TapDetector tapDetector; }; diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index 786c23d..8081ca5 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2021 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -33,13 +33,10 @@ #include "network/network.h" #include #include "calibration.h" -#include "configuration.h" #include "GlobalVars.h" void MPU6050Sensor::motionSetup() { - //DeviceConfig * const config = getConfigPtr(); - imu.initialize(addr); if (!imu.testConnection()) { @@ -49,6 +46,28 @@ void MPU6050Sensor::motionSetup() m_Logger.info("Connected to MPU6050 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr); +#ifndef IMU_MPU6050_RUNTIME_CALIBRATION + // Initialize the configuration + { + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::MPU6050: + m_Calibration = sensorCalibration.data.mpu6050; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + } +#endif + devStatus = imu.dmpInitialize(); if (devStatus == 0) @@ -157,28 +176,30 @@ void MPU6050Sensor::startCalibration(int calibrationType) { m_Logger.debug("Gathered baseline gyro reading"); m_Logger.debug("Starting offset finder"); - DeviceConfig *const config = getConfigPtr(); - switch (calibrationType) { case CALIBRATION_TYPE_INTERNAL_ACCEL: imu.CalibrateAccel(10); Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_ACCEL, 0);//doesn't send calibration data anymore, has that been depricated in server? - config->calibration->A_B[0] = imu.getXAccelOffset(); - config->calibration->A_B[1] = imu.getYAccelOffset(); - config->calibration->A_B[2] = imu.getZAccelOffset(); - saveConfig(); + m_Calibration.A_B[0] = imu.getXAccelOffset(); + m_Calibration.A_B[1] = imu.getYAccelOffset(); + m_Calibration.A_B[2] = imu.getZAccelOffset(); break; case CALIBRATION_TYPE_INTERNAL_GYRO: imu.CalibrateGyro(10); Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_GYRO, 0);//doesn't send calibration data anymore - config->calibration->G_off[0] = imu.getXGyroOffset(); - config->calibration->G_off[1] = imu.getYGyroOffset(); - config->calibration->G_off[2] = imu.getZGyroOffset(); - saveConfig(); + m_Calibration.G_off[0] = imu.getXGyroOffset(); + m_Calibration.G_off[1] = imu.getYGyroOffset(); + m_Calibration.G_off[2] = imu.getZGyroOffset(); break; } + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::MPU6050; + calibration.data.mpu6050 = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + m_Logger.info("Calibration finished"); #endif // !IMU_MPU6050_RUNTIME_CALIBRATION diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index ad5aac3..e8ed8e2 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -46,6 +46,10 @@ private: uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]{}; // FIFO storage buffer + +#ifndef IMU_MPU6050_RUNTIME_CALIBRATION + SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration; +#endif }; #endif diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 1345c88..2cf334d 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -1,6 +1,7 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain, S.J. Remington, SlimeVR contributors + Copyright (c) 2021 Eiren Rain, S.J. Remington & SlimeVR contributors + Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights @@ -26,8 +27,8 @@ #include "calibration.h" #include "magneto1.4.h" #include "GlobalVars.h" -// #include "mahony.h" -// #include "madgwick.h" +#include "mahony.h" +#include "madgwick.h" #if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_)) #include "dmpmag.h" #endif @@ -38,8 +39,6 @@ constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB #define MAG_CORR_RATIO 0.2 void MPU9250Sensor::motionSetup() { - DeviceConfig * const config = getConfigPtr(); - calibration = &config->calibration[sensorId]; // initialize device imu.initialize(addr); if(!imu.testConnection()) { @@ -68,6 +67,27 @@ void MPU9250Sensor::motionSetup() { startCalibration(0); } } + + // Initialize the configuration + { + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::MPU9250: + m_Calibration = sensorCalibration.data.mpu9250; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + } + #if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_)) devStatus = imu.dmpInitialize(); if(devStatus == 0){ @@ -170,9 +190,9 @@ void MPU9250Sensor::getMPUScaled() #if defined(_MAHONY_H_) || defined(_MADGWICK_H_) int16_t ax, ay, az, gx, gy, gz, mx, my, mz; imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - Gxyz[0] = ((float)gx - calibration->G_off[0]) * gscale; //250 LSB(d/s) default to radians/s - Gxyz[1] = ((float)gy - calibration->G_off[1]) * gscale; - Gxyz[2] = ((float)gz - calibration->G_off[2]) * gscale; + Gxyz[0] = ((float)gx - m_Calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s + Gxyz[1] = ((float)gy - m_Calibration.G_off[1]) * gscale; + Gxyz[2] = ((float)gz - m_Calibration.G_off[2]) * gscale; Axyz[0] = (float)ax; Axyz[1] = (float)ay; @@ -181,13 +201,13 @@ void MPU9250Sensor::getMPUScaled() //apply offsets (bias) and scale factors from Magneto #if useFullCalibrationMatrix == true for (i = 0; i < 3; i++) - temp[i] = (Axyz[i] - calibration->A_B[i]); - Axyz[0] = calibration->A_Ainv[0][0] * temp[0] + calibration->A_Ainv[0][1] * temp[1] + calibration->A_Ainv[0][2] * temp[2]; - Axyz[1] = calibration->A_Ainv[1][0] * temp[0] + calibration->A_Ainv[1][1] * temp[1] + calibration->A_Ainv[1][2] * temp[2]; - Axyz[2] = calibration->A_Ainv[2][0] * temp[0] + calibration->A_Ainv[2][1] * temp[1] + calibration->A_Ainv[2][2] * temp[2]; + temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + Axyz[0] = m_Calibration.A_Ainv[0][0] * temp[0] + m_Calibration.A_Ainv[0][1] * temp[1] + m_Calibration.A_Ainv[0][2] * temp[2]; + Axyz[1] = m_Calibration.A_Ainv[1][0] * temp[0] + m_Calibration.A_Ainv[1][1] * temp[1] + m_Calibration.A_Ainv[1][2] * temp[2]; + Axyz[2] = m_Calibration.A_Ainv[2][0] * temp[0] + m_Calibration.A_Ainv[2][1] * temp[1] + m_Calibration.A_Ainv[2][2] * temp[2]; #else for (i = 0; i < 3; i++) - Axyz[i] = (Axyz[i] - calibration->A_B[i]); + Axyz[i] = (Axyz[i] - m-Calibration.A_B[i]); #endif #else @@ -205,13 +225,13 @@ void MPU9250Sensor::getMPUScaled() //apply offsets and scale factors from Magneto #if useFullCalibrationMatrix == true for (i = 0; i < 3; i++) - temp[i] = (Mxyz[i] - calibration->M_B[i]); - Mxyz[0] = calibration->M_Ainv[0][0] * temp[0] + calibration->M_Ainv[0][1] * temp[1] + calibration->M_Ainv[0][2] * temp[2]; - Mxyz[1] = calibration->M_Ainv[1][0] * temp[0] + calibration->M_Ainv[1][1] * temp[1] + calibration->M_Ainv[1][2] * temp[2]; - Mxyz[2] = calibration->M_Ainv[2][0] * temp[0] + calibration->M_Ainv[2][1] * temp[1] + calibration->M_Ainv[2][2] * temp[2]; + temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + Mxyz[0] = m_Calibration.M_Ainv[0][0] * temp[0] + m_Calibration.M_Ainv[0][1] * temp[1] + m_Calibration.M_Ainv[0][2] * temp[2]; + Mxyz[1] = m_Calibration.M_Ainv[1][0] * temp[0] + m_Calibration.M_Ainv[1][1] * temp[1] + m_Calibration.M_Ainv[1][2] * temp[2]; + Mxyz[2] = m_Calibration.M_Ainv[2][0] * temp[0] + m_Calibration.M_Ainv[2][1] * temp[1] + m_Calibration.M_Ainv[2][2] * temp[2]; #else for (i = 0; i < 3; i++) - Mxyz[i] = (Mxyz[i] - calibration->M_B[i]); + Mxyz[i] = (Mxyz[i] - m_Calibration.M_B[i]); #endif } @@ -220,7 +240,7 @@ void MPU9250Sensor::startCalibration(int calibrationType) { #if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_)) // with DMP, we just need mag data constexpr int calibrationSamples = 300; - DeviceConfig *config = getConfigPtr(); + // Blink calibrating led before user should rotate the sensor m_Logger.info("Gently rotate the device while it's gathering magnetometer data"); ledManager.pattern(15, 300, 3000/310); @@ -245,19 +265,18 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.debug("[INFO] Magnetometer calibration matrix:"); m_Logger.debug("{"); for (int i = 0; i < 3; i++) { - config->calibration[sensorId].M_B[i] = M_BAinv[0][i]; - config->calibration[sensorId].M_Ainv[0][i] = M_BAinv[1][i]; - config->calibration[sensorId].M_Ainv[1][i] = M_BAinv[2][i]; - config->calibration[sensorId].M_Ainv[2][i] = M_BAinv[3][i]; + m_Calibration.M_B[i] = M_BAinv[0][i]; + m_Calibration.M_Ainv[0][i] = M_BAinv[1][i]; + m_Calibration.M_Ainv[1][i] = M_BAinv[2][i]; + m_Calibration.M_Ainv[2][i] = M_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); } m_Logger.debug("}"); -#elif +#else m_Logger.debug("Gathering raw data for device calibration..."); constexpr int calibrationSamples = 300; - DeviceConfig *config = getConfigPtr(); // Reset values Gxyz[0] = 0; Gxyz[1] = 0; @@ -283,9 +302,9 @@ void MPU9250Sensor::startCalibration(int calibrationType) { #endif Network::sendRawCalibrationData(Gxyz, CALIBRATION_TYPE_EXTERNAL_GYRO, 0); - config->calibration[sensorId].G_off[0] = Gxyz[0]; - config->calibration[sensorId].G_off[1] = Gxyz[1]; - config->calibration[sensorId].G_off[2] = Gxyz[2]; + m_Calibration.G_off[0] = Gxyz[0]; + m_Calibration.G_off[1] = Gxyz[1]; + m_Calibration.G_off[2] = Gxyz[2]; // Blink calibrating led before user should rotate the sensor m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data"); @@ -320,29 +339,36 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.debug("{"); for (int i = 0; i < 3; i++) { - config->calibration[sensorId].A_B[i] = A_BAinv[0][i]; - config->calibration[sensorId].A_Ainv[0][i] = A_BAinv[1][i]; - config->calibration[sensorId].A_Ainv[1][i] = A_BAinv[2][i]; - config->calibration[sensorId].A_Ainv[2][i] = A_BAinv[3][i]; + m_Calibration.A_B[i] = A_BAinv[0][i]; + m_Calibration.A_Ainv[0][i] = A_BAinv[1][i]; + m_Calibration.A_Ainv[1][i] = A_BAinv[2][i]; + m_Calibration.A_Ainv[2][i] = A_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); } m_Logger.debug("}"); m_Logger.debug("[INFO] Magnetometer calibration matrix:"); m_Logger.debug("{"); for (int i = 0; i < 3; i++) { - config->calibration[sensorId].M_B[i] = M_BAinv[0][i]; - config->calibration[sensorId].M_Ainv[0][i] = M_BAinv[1][i]; - config->calibration[sensorId].M_Ainv[1][i] = M_BAinv[2][i]; - config->calibration[sensorId].M_Ainv[2][i] = M_BAinv[3][i]; + m_Calibration.M_B[i] = M_BAinv[0][i]; + m_Calibration.M_Ainv[0][i] = M_BAinv[1][i]; + m_Calibration.M_Ainv[1][i] = M_BAinv[2][i]; + m_Calibration.M_Ainv[2][i] = M_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); } m_Logger.debug("}"); #endif - m_Logger.debug("Now Saving EEPROM"); - setConfig(*config); + m_Logger.debug("Saving the calibration data"); + + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::MPU9250; + calibration.data.mpu9250 = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + ledManager.off(); Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0); - m_Logger.debug("Finished Saving EEPROM"); + m_Logger.debug("Saved the calibration data"); + m_Logger.info("Calibration data gathered"); } \ No newline at end of file diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index 1e7eae0..7419b47 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -41,7 +41,6 @@ public: private: MPU9250 imu{}; - CalibrationConfig *calibration; bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) @@ -59,6 +58,8 @@ private: // Loop timing globals unsigned long now = 0, last = 0; // micros() timers float deltat = 0; // loop time in seconds + + SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration; }; #endif diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index d26fb7b..bfb63e6 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -27,7 +27,7 @@ #include #include #include -#include "configuration.h" +#include "configuration/Configuration.h" #include "globals.h" #include "logging/Logger.h" #include "utils.h" diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 29b15b5..0b37d51 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + Copyright (c) 2021 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -25,7 +25,7 @@ #include "network/network.h" #include "logging/Logger.h" #include -#include +#include "GlobalVars.h" namespace SerialCommands { SlimeVR::Logging::Logger logger("SerialCommands"); @@ -73,9 +73,9 @@ namespace SerialCommands { void cmdFactoryReset(CmdParser * parser) { logger.info("FACTORY RESET"); - for (int i = 0; i <= 4096; i++) // Clear EEPROM - EEPROM.write(i, 0xFF); - EEPROM.commit(); + + configuration.reset(); + WiFi.disconnect(true); // Clear WiFi credentials #if ESP8266 ESP.eraseConfig(); // Clear ESP config diff --git a/src/utils.h b/src/utils.h index 3fa1fef..c009432 100644 --- a/src/utils.h +++ b/src/utils.h @@ -1,7 +1,31 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + #ifndef UTILS_H #define UTILS_H #define UNPACK_VECTOR(V) V.x, V.y, V.z +#define UNPACK_VECTOR_ARRAY(V) V[0], V[1], V[2] #define UNPACK_QUATERNION(Q) Q.x, Q.y, Q.z, Q.w #endif diff --git a/test/i2cscan.cpp b/test/i2cscan.cpp index 66f0e47..e09caf9 100644 --- a/test/i2cscan.cpp +++ b/test/i2cscan.cpp @@ -9,12 +9,12 @@ #include #include "ota.h" -#include "configuration.h" +#include "configuration/Configuration.h" #include "helper_3dmath.h" #include "udpclient.h" #include "credentials.h" -DeviceConfig config; +SlimeVR::Configuration::DeviceConfig config; uint8_t portArray[] = {16, 5, 4, 0, 2, 14, 12, 13}; //String portMap[] = {"D0", "D1", "D2", "D3", "D4", "D5", "D6", "D7"}; //for Wemos