From 85dead25f339bc3a6d79a147b6ca0e9e55741f1d Mon Sep 17 00:00:00 2001 From: Butterscotch! Date: Wed, 30 Oct 2024 14:15:43 -0400 Subject: [PATCH 1/2] Change "PASS" to "PASSWD" (#343) Co-authored-by: Eiren Rain --- src/serial/serialcommands.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 06c427942..3ef57fb59 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -250,7 +250,7 @@ namespace SerialCommands { for (int i = 0; i < scanRes; i++) { logger.info("[WSCAN] %d:\t%02d\t%s\t(%d)\t%s", i, WiFi.SSID(i).length(), WiFi.SSID(i).c_str(), WiFi.RSSI(i), - ((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASS") + ((WiFi.encryptionType(i) == 0) ? "OPEN" : "PASSWD") ); } WiFi.scanDelete(); From a9f5b1ae8c65e13856d14dacdef5dc341e42f7b5 Mon Sep 17 00:00:00 2001 From: Uriel Date: Wed, 30 Oct 2024 19:23:49 +0100 Subject: [PATCH 2/2] Add a way to toggle magnetometer in runtime (#341) * Untested magnetometer toggle feature for BNO08X and overal packet structure for setting flags from the server * Some build fixes * refactor(configuration): rename `CalibrationConfig` to `SensorConfig` * fix network package order * typo found * ignore clion files * finish feature * remove ota config that i used * C skill issue on defines * i have personal issues with C * do a reset before * reinit sensor * Fix remaining merge errors * remove BNO_USE_MAGNETOMETER_CORRECTION * Update src/sensors/sensor.h Co-authored-by: Lena <25586367+Vyolex@users.noreply.github.com> * who loves tabs * send sensorconfig instead of magdata on sensorinfo * Bump protocol and firmware version --------- Co-authored-by: Eiren Rain Co-authored-by: DevMiner Co-authored-by: Lena <25586367+Vyolex@users.noreply.github.com> --- .gitignore | 1 + src/configuration/CalibrationConfig.cpp | 47 ----- src/configuration/CalibrationConfig.h | 141 ------------- src/configuration/Configuration.cpp | 73 ++++--- src/configuration/Configuration.h | 10 +- src/configuration/DeviceConfig.h | 48 ++--- src/configuration/SensorConfig.cpp | 64 ++++++ src/configuration/SensorConfig.h | 155 ++++++++++++++ src/debug.h | 5 +- .../GyroTemperatureCalibrator.h | 16 +- src/network/connection.cpp | 59 +++++- src/network/connection.h | 2 + src/network/featureflags.h | 140 ++++++------- src/network/packets.h | 3 + src/sensors/bmi160sensor.cpp | 92 ++++----- src/sensors/bmi160sensor.h | 6 +- src/sensors/bno055sensor.h | 3 +- src/sensors/bno080sensor.cpp | 135 ++++++------ src/sensors/bno080sensor.h | 4 + src/sensors/icm20948sensor.cpp | 64 +++--- src/sensors/icm20948sensor.h | 2 +- src/sensors/mpu6050sensor.cpp | 26 +-- src/sensors/mpu6050sensor.h | 2 +- src/sensors/mpu9250sensor.cpp | 66 +++--- src/sensors/mpu9250sensor.h | 2 +- src/sensors/sensor.cpp | 5 + src/sensors/sensor.h | 193 ++++++++++-------- src/sensors/softfusion/softfusionsensor.h | 16 +- 28 files changed, 745 insertions(+), 635 deletions(-) delete mode 100644 src/configuration/CalibrationConfig.cpp delete mode 100644 src/configuration/CalibrationConfig.h create mode 100644 src/configuration/SensorConfig.cpp create mode 100644 src/configuration/SensorConfig.h diff --git a/.gitignore b/.gitignore index e60545875..2f8cc9b8a 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ build/ venv/ cache/ +.idea/ diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp deleted file mode 100644 index 0c9458df1..000000000 --- a/src/configuration/CalibrationConfig.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ - -#include "CalibrationConfig.h" - -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"; - case SFUSION: - return "SoftFusion (common)"; - default: - return "UNKNOWN"; - } - } - } -} diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h deleted file mode 100644 index f07e0dc2f..000000000 --- a/src/configuration/CalibrationConfig.h +++ /dev/null @@ -1,141 +0,0 @@ -/* - 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 -#include "consts.h" - -namespace SlimeVR { - namespace Configuration { - struct BMI160CalibrationConfig { - // 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]; - - // calibration temperature for dynamic compensation - float temperature; - }; - - struct SoftFusionCalibrationConfig { - ImuID ImuType; - uint16_t MotionlessDataLen; - - // 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]; - - // calibration temperature for dynamic compensation - float temperature; - - // real measured sensor sampling rate - float A_Ts; - float G_Ts; - float M_Ts; - - // gyro sensitivity multiplier - float G_Sens[3]; - - uint8_t MotionlessData[60]; - }; - - - 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]; - }; - - struct ICM42688CalibrationConfig { - // 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]; - }; - - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, SFUSION }; - - const char* calibrationConfigTypeToString(CalibrationConfigType type); - - struct CalibrationConfig { - CalibrationConfigType type; - - union { - BMI160CalibrationConfig bmi160; - SoftFusionCalibrationConfig sfusion; - MPU6050CalibrationConfig mpu6050; - MPU9250CalibrationConfig mpu9250; - ICM20948CalibrationConfig icm20948; - } data; - }; - } -} - -#endif diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 0114c30c0..482dd7204 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -82,7 +82,7 @@ namespace SlimeVR { save(); } - loadCalibrations(); + loadSensors(); m_Loaded = true; @@ -94,19 +94,19 @@ namespace SlimeVR { } void Configuration::save() { - for (size_t i = 0; i < m_Calibrations.size(); i++) { - CalibrationConfig config = m_Calibrations[i]; - if (config.type == CalibrationConfigType::NONE) { + for (size_t i = 0; i < m_Sensors.size(); i++) { + SensorConfig config = m_Sensors[i]; + if (config.type == SensorConfigType::NONE) { continue; } char path[17]; sprintf(path, DIR_CALIBRATIONS"/%d", i); - m_Logger.trace("Saving calibration data for %d", i); + m_Logger.trace("Saving sensor config data for %d", i); File file = LittleFS.open(path, "w"); - file.write((uint8_t*)&config, sizeof(CalibrationConfig)); + file.write((uint8_t*)&config, sizeof(SensorConfig)); file.close(); } @@ -122,7 +122,7 @@ namespace SlimeVR { void Configuration::reset() { LittleFS.format(); - m_Calibrations.clear(); + m_Sensors.clear(); m_Config.version = 1; save(); @@ -133,41 +133,41 @@ namespace SlimeVR { return m_Config.version; } - size_t Configuration::getCalibrationCount() const { - return m_Calibrations.size(); + size_t Configuration::getSensorCount() const { + return m_Sensors.size(); } - CalibrationConfig Configuration::getCalibration(size_t sensorID) const { - if (sensorID >= m_Calibrations.size()) { + SensorConfig Configuration::getSensor(size_t sensorID) const { + if (sensorID >= m_Sensors.size()) { return {}; } - return m_Calibrations.at(sensorID); + return m_Sensors.at(sensorID); } - void Configuration::setCalibration(size_t sensorID, const CalibrationConfig& config) { - size_t currentCalibrations = m_Calibrations.size(); + void Configuration::setSensor(size_t sensorID, const SensorConfig& config) { + size_t currentSensors = m_Sensors.size(); - if (sensorID >= currentCalibrations) { - m_Calibrations.resize(sensorID + 1); + if (sensorID >= currentSensors) { + m_Sensors.resize(sensorID + 1); } - m_Calibrations[sensorID] = config; + m_Sensors[sensorID] = config; } - void Configuration::loadCalibrations() { + void Configuration::loadSensors() { SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) { - CalibrationConfig calibrationConfig; - f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig)); + SensorConfig sensorConfig; + f.read((uint8_t*)&sensorConfig, sizeof(SensorConfig)); uint8_t sensorId = strtoul(f.name(), nullptr, 10); m_Logger.debug( "Found sensor calibration for %s at index %d", - calibrationConfigTypeToString(calibrationConfig.type), + calibrationConfigTypeToString(sensorConfig.type), sensorId ); - setCalibration(sensorId, calibrationConfig); + setSensor(sensorId, sensorConfig); }); } @@ -200,8 +200,8 @@ namespace SlimeVR { return false; } - CalibrationConfigType storedConfigType; - f.read((uint8_t*)&storedConfigType, sizeof(CalibrationConfigType)); + SensorConfigType storedConfigType; + f.read((uint8_t*)&storedConfigType, sizeof(SensorConfigType)); if (storedConfigType != config.type) { m_Logger.debug( @@ -225,7 +225,7 @@ namespace SlimeVR { } bool Configuration::saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config) { - if (config.type == CalibrationConfigType::NONE) { + if (config.type == SensorConfigType::NONE) { return false; } @@ -249,17 +249,17 @@ namespace SlimeVR { void Configuration::print() { m_Logger.info("Configuration:"); m_Logger.info(" Version: %d", m_Config.version); - m_Logger.info(" %d Calibrations:", m_Calibrations.size()); + m_Logger.info(" %d Sensors:", m_Sensors.size()); - for (size_t i = 0; i < m_Calibrations.size(); i++) { - const CalibrationConfig& c = m_Calibrations[i]; + for (size_t i = 0; i < m_Sensors.size(); i++) { + const SensorConfig& c = m_Sensors[i]; m_Logger.info(" - [%3d] %s", i, calibrationConfigTypeToString(c.type)); switch (c.type) { - case CalibrationConfigType::NONE: + case SensorConfigType::NONE: break; - case CalibrationConfigType::BMI160: + case SensorConfigType::BMI160: m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.bmi160.A_B)); m_Logger.info(" A_Ainv :"); @@ -272,7 +272,7 @@ namespace SlimeVR { break; - case CalibrationConfigType::SFUSION: + case SensorConfigType::SFUSION: m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B)); m_Logger.info(" A_Ainv :"); @@ -284,14 +284,14 @@ namespace SlimeVR { m_Logger.info(" Temperature: %f", c.data.sfusion.temperature); break; - case CalibrationConfigType::ICM20948: + case SensorConfigType::ICM20948: m_Logger.info(" G: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.G)); m_Logger.info(" A: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.A)); m_Logger.info(" C: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.C)); break; - case CalibrationConfigType::MPU9250: + case SensorConfigType::MPU9250: m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu9250.A_B)); m_Logger.info(" A_Ainv:"); @@ -310,11 +310,16 @@ namespace SlimeVR { break; - case CalibrationConfigType::MPU6050: + case SensorConfigType::MPU6050: m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)); m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)); break; + + case SensorConfigType::BNO0XX: + m_Logger.info(" magEnabled: %d", c.data.bno0XX.magEnabled); + + break; } } } diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h index 4a0eb69e3..c54ca0168 100644 --- a/src/configuration/Configuration.h +++ b/src/configuration/Configuration.h @@ -43,21 +43,21 @@ namespace SlimeVR { int32_t getVersion() const; - size_t getCalibrationCount() const; - CalibrationConfig getCalibration(size_t sensorID) const; - void setCalibration(size_t sensorID, const CalibrationConfig& config); + size_t getSensorCount() const; + SensorConfig getSensor(size_t sensorID) const; + void setSensor(size_t sensorID, const SensorConfig& config); bool loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config); bool saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config); private: - void loadCalibrations(); + void loadSensors(); bool runMigrations(int32_t version); bool m_Loaded = false; DeviceConfig m_Config{}; - std::vector m_Calibrations; + std::vector m_Sensors; Logging::Logger m_Logger = Logging::Logger("Configuration"); }; diff --git a/src/configuration/DeviceConfig.h b/src/configuration/DeviceConfig.h index b708ca73b..61a2327f0 100644 --- a/src/configuration/DeviceConfig.h +++ b/src/configuration/DeviceConfig.h @@ -1,37 +1,33 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_CONFIGURATION_DEVICECONFIG_H #define SLIMEVR_CONFIGURATION_DEVICECONFIG_H -#include "CalibrationConfig.h" - -namespace SlimeVR { - namespace Configuration { - struct DeviceConfig { - int32_t version; - }; - } -} +namespace SlimeVR::Configuration { +struct DeviceConfig { + int32_t version; +}; +} // namespace SlimeVR::Configuration #endif diff --git a/src/configuration/SensorConfig.cpp b/src/configuration/SensorConfig.cpp new file mode 100644 index 000000000..cff2415d4 --- /dev/null +++ b/src/configuration/SensorConfig.cpp @@ -0,0 +1,64 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "SensorConfig.h" + +namespace SlimeVR { +namespace Configuration { +const char* calibrationConfigTypeToString(SensorConfigType type) { + switch (type) { + case SensorConfigType::NONE: + return "NONE"; + case SensorConfigType::BMI160: + return "BMI160"; + case SensorConfigType::MPU6050: + return "MPU6050"; + case SensorConfigType::MPU9250: + return "MPU9250"; + case SensorConfigType::ICM20948: + return "ICM20948"; + case SensorConfigType::SFUSION: + return "SoftFusion (common)"; + case SensorConfigType::BNO0XX: + return "BNO0XX"; + default: + return "UNKNOWN"; + } +} + +uint16_t configDataToNumber(SensorConfig sensorConfig) { + uint16_t data = 0; + switch (sensorConfig.type) { + case SensorConfigType::BNO0XX: { + auto config = &sensorConfig.data.bno0XX; + data += config->magEnabled; + break; + } + case SensorConfigType::NONE: + default: + break; + } + return data; +} +} // namespace Configuration +} // namespace SlimeVR diff --git a/src/configuration/SensorConfig.h b/src/configuration/SensorConfig.h new file mode 100644 index 000000000..f63c5db69 --- /dev/null +++ b/src/configuration/SensorConfig.h @@ -0,0 +1,155 @@ +/* + 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_SENSORCONFIG_H +#define SLIMEVR_CONFIGURATION_SENSORCONFIG_H + +#include +#include "consts.h" + +namespace SlimeVR { +namespace Configuration { +struct BMI160SensorConfig { + // 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]; + + // calibration temperature for dynamic compensation + float temperature; +}; + +struct SoftFusionSensorConfig { + ImuID ImuType; + uint16_t MotionlessDataLen; + + // 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]; + + // calibration temperature for dynamic compensation + float temperature; + + // real measured sensor sampling rate + float A_Ts; + float G_Ts; + float M_Ts; + + // gyro sensitivity multiplier + float G_Sens[3]; + + uint8_t MotionlessData[60]; +}; + +struct MPU6050SensorConfig { + // accelerometer offsets and correction matrix + float A_B[3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; +}; + +struct MPU9250SensorConfig { + // 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 ICM20948SensorConfig { + // gyroscope bias + int32_t G[3]; + + // accelerometer bias + int32_t A[3]; + + // compass bias + int32_t C[3]; +}; + +struct ICM42688SensorConfig { + // 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 BNO0XXSensorConfig { + bool magEnabled; +}; + +enum class SensorConfigType { + NONE, + BMI160, + MPU6050, + MPU9250, + ICM20948, + SFUSION, + BNO0XX +}; + +const char* calibrationConfigTypeToString(SensorConfigType type); + +struct SensorConfig { + SensorConfigType type; + + union { + BMI160SensorConfig bmi160; + SoftFusionSensorConfig sfusion; + MPU6050SensorConfig mpu6050; + MPU9250SensorConfig mpu9250; + ICM20948SensorConfig icm20948; + BNO0XXSensorConfig bno0XX; + } data; +}; + +uint16_t configDataToNumber(SensorConfig sensorConfig); +} // namespace Configuration +} // namespace SlimeVR + +#endif diff --git a/src/debug.h b/src/debug.h index f08260b89..5e794f26b 100644 --- a/src/debug.h +++ b/src/debug.h @@ -27,7 +27,6 @@ #define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration #define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs -#define BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true. #define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO0xx currently) #define LOAD_BIAS true // Loads the bias values from NVS on start #define SAVE_BIAS true // Periodically saves bias calibration data to NVS @@ -90,7 +89,7 @@ // Not recommended for production #define ENABLE_INSPECTION false -#define PROTOCOL_VERSION 17 -#define FIRMWARE_VERSION "0.4.0" +#define PROTOCOL_VERSION 18 +#define FIRMWARE_VERSION "0.5.0" #endif // SLIMEVR_DEBUG_H_ diff --git a/src/motionprocessing/GyroTemperatureCalibrator.h b/src/motionprocessing/GyroTemperatureCalibrator.h index eb27f7fb9..1757a0d2c 100644 --- a/src/motionprocessing/GyroTemperatureCalibrator.h +++ b/src/motionprocessing/GyroTemperatureCalibrator.h @@ -28,7 +28,7 @@ #include #include "debug.h" #include "../logging/Logger.h" -#include "../configuration/CalibrationConfig.h" +#include "../configuration/SensorConfig.h" #include "OnlinePolyfit.h" @@ -101,7 +101,7 @@ struct GyroTemperatureOffsetSample { }; struct GyroTemperatureCalibrationConfig { - SlimeVR::Configuration::CalibrationConfigType type; + SlimeVR::Configuration::SensorConfigType type; float sensitivityLSB; float minTemperatureRange; @@ -115,13 +115,13 @@ struct GyroTemperatureCalibrationConfig { float cz[4] = {0.0}; bool hasCoeffs = false; - GyroTemperatureCalibrationConfig(SlimeVR::Configuration::CalibrationConfigType _type, float _sensitivityLSB) : + GyroTemperatureCalibrationConfig(SlimeVR::Configuration::SensorConfigType _type, float _sensitivityLSB) : type(_type), sensitivityLSB(_sensitivityLSB), minTemperatureRange(1000), maxTemperatureRange(-1000) { } - + bool hasData() { return minTemperatureRange != 1000; } @@ -164,13 +164,13 @@ class GyroTemperatureCalibrator { public: uint8_t sensorId; GyroTemperatureCalibrationConfig config; - + // set when config is fully calibrated is saved OR on startup when loaded config is fully calibrated; // left unset when sending saving command over serial so it can continue calibration and autosave later bool configSaved = false; bool configSaveFailed = false; - GyroTemperatureCalibrator(SlimeVR::Configuration::CalibrationConfigType _configType, uint8_t _sensorId, float sensitivity, uint32_t _samplesPerStep): + GyroTemperatureCalibrator(SlimeVR::Configuration::SensorConfigType _configType, uint8_t _sensorId, float sensitivity, uint32_t _samplesPerStep): sensorId(_sensorId), config(_configType, sensitivity), samplesPerStep(_samplesPerStep), @@ -200,7 +200,7 @@ class GyroTemperatureCalibrator { GyroTemperatureCalibrationState state; uint32_t samplesPerStep; SlimeVR::Logging::Logger m_Logger; - + float lastApproximatedTemperature = 0.0f; float lastApproximatedOffsets[3]; @@ -216,4 +216,4 @@ class GyroTemperatureCalibrator { void resetCurrentTemperatureState(); }; -#endif \ No newline at end of file +#endif diff --git a/src/network/connection.cpp b/src/network/connection.cpp index be7e74feb..8796be775 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -89,7 +89,7 @@ bool Connection::endPacket() { MUST_TRANSFER_BOOL((innerPacketSize > 0)); m_IsBundle = false; - + if (m_BundlePacketInnerCount == 0) { sendPacketType(PACKET_BUNDLE); sendPacketNumber(); @@ -128,7 +128,7 @@ bool Connection::endBundle() { MUST_TRANSFER_BOOL(m_IsBundle); m_IsBundle = false; - + MUST_TRANSFER_BOOL((m_BundlePacketInnerCount > 0)); return endPacket(); @@ -298,6 +298,7 @@ void Connection::sendSensorInfo(Sensor& sensor) { MUST(sendByte(sensor.getSensorId())); MUST(sendByte(static_cast(sensor.getSensorState()))); MUST(sendByte(static_cast(sensor.getSensorType()))); + MUST(sendShort(sensor.getSensorConfigData())); MUST(endPacket()); } @@ -381,6 +382,21 @@ void Connection::sendFeatureFlags() { MUST(endPacket()); } +// PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24 + +void Connection::sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_ACKNOWLEDGE_CONFIG_CHANGE)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendShort(configType)); + + MUST(endPacket()); +} + void Connection::sendTrackerDiscovery() { MUST(!m_Connected); @@ -525,7 +541,7 @@ void Connection::updateSensorState(std::vector> & sensor } } -void Connection::maybeRequestFeatureFlags() { +void Connection::maybeRequestFeatureFlags() { if (m_ServerFeatures.isAvailable() || m_FeatureFlagsRequestAttempts >= 15) { return; } @@ -557,6 +573,8 @@ void Connection::searchForServer() { m_UDP.remotePort() ); m_Logger.traceArray("UDP packet contents: ", m_Packet, len); +#else + (void)len; #endif // Handshake is different, it has 3 in the first byte, not the 4th, and data @@ -571,7 +589,7 @@ void Connection::searchForServer() { m_ServerPort = m_UDP.remotePort(); m_LastPacketTimestamp = millis(); m_Connected = true; - + m_FeatureFlagsRequestAttempts = 0; m_ServerFeatures = ServerFeatures { }; @@ -689,7 +707,7 @@ void Connection::update() { break; - case PACKET_FEATURE_FLAGS: + case PACKET_FEATURE_FLAGS: { // Packet type (4) + Packet number (8) + flags (len - 12) if (len < 13) { m_Logger.warn("Invalid feature flags packet: too short"); @@ -697,7 +715,7 @@ void Connection::update() { } bool hadFlags = m_ServerFeatures.isAvailable(); - + uint32_t flagsLength = len - 12; m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength); @@ -710,6 +728,35 @@ void Connection::update() { } break; + } + + case PACKET_SET_CONFIG_FLAG: { + // Packet type (4) + Packet number (8) + sensor_id(1) + flag_id (2) + state (1) + if (len < 16) { + m_Logger.warn("Invalid sensor config flag packet: too short"); + break; + } + uint8_t sensorId = m_Packet[12]; + uint16_t flagId = m_Packet[13] << 8 | m_Packet[14]; + bool newState = m_Packet[15] > 0; + if(sensorId == UINT8_MAX) { + for (auto& sensor : sensors) { + sensor->setFlag(flagId, newState); + } + } else { + auto & sensors = sensorManager.getSensors(); + if(sensorId < sensors.size()) { + auto& sensor = sensors[sensorId]; + sensor->setFlag(flagId, newState); + } else { + m_Logger.warn("Invalid sensor config flag packet: invalid sensor id"); + break; + } + } + sendAcknowledgeConfigChange(sensorId, flagId); + configuration.save(); + break; + } } } diff --git a/src/network/connection.h b/src/network/connection.h index b0aefc3e5..c20231691 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -158,6 +158,8 @@ class Connection { // PACKET_SENSOR_INFO 15 void sendSensorInfo(Sensor& sensor); + void sendAcknowledgeConfigChange(uint8_t sensorId, uint16_t configType); + bool m_Connected = false; SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); diff --git a/src/network/featureflags.h b/src/network/featureflags.h index b0820be40..f9e1dfa1f 100644 --- a/src/network/featureflags.h +++ b/src/network/featureflags.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 SlimeVR Contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + 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_FEATURE_FLAGS_H_ @@ -34,70 +34,70 @@ */ struct ServerFeatures { public: - enum EServerFeatureFlags: uint32_t { - // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). - PROTOCOL_BUNDLE_SUPPORT, - - // Add new flags here - - BITS_TOTAL, - }; - - bool has(EServerFeatureFlags flag) { - uint32_t bit = static_cast(flag); - return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8))); - } - - /** - * Whether the server supports the "feature flags" feature, - * set to true when we've received flags packet from the server. - */ - bool isAvailable() { - return m_Available; - } - - static ServerFeatures from(uint8_t* received, uint32_t length) { - ServerFeatures res; - res.m_Available = true; - memcpy(res.m_Flags, received, std::min(static_cast(sizeof(res.m_Flags)), length)); - return res; - } + enum EServerFeatureFlags: uint32_t { + // Server can parse bundle packets: `PACKET_BUNDLE` = 100 (0x64). + PROTOCOL_BUNDLE_SUPPORT, + + // Add new flags here + + BITS_TOTAL, + }; + + bool has(EServerFeatureFlags flag) { + uint32_t bit = static_cast(flag); + return m_Available && (m_Flags[bit / 8] & (1 << (bit % 8))); + } + + /** + * Whether the server supports the "feature flags" feature, + * set to true when we've received flags packet from the server. + */ + bool isAvailable() { + return m_Available; + } + + static ServerFeatures from(uint8_t* received, uint32_t length) { + ServerFeatures res; + res.m_Available = true; + memcpy(res.m_Flags, received, std::min(static_cast(sizeof(res.m_Flags)), length)); + return res; + } private: - bool m_Available = false; + bool m_Available = false; - uint8_t m_Flags[static_cast(EServerFeatureFlags::BITS_TOTAL) / 8 + 1]; + uint8_t m_Flags[static_cast(EServerFeatureFlags::BITS_TOTAL) / 8 + 1]; }; class FirmwareFeatures { public: - enum EFirmwareFeatureFlags: uint32_t { - // EXAMPLE_FEATURE, + enum EFirmwareFeatureFlags: uint32_t { + // EXAMPLE_FEATURE, B64_WIFI_SCANNING = 1, + SENSOR_CONFIG = 2, + // Add new flags here - // Add new flags here + BITS_TOTAL, + }; - BITS_TOTAL, - }; - - // Flags to send - static constexpr const std::initializer_list flagsEnabled = { - // EXAMPLE_FEATURE, + // Flags to send + static constexpr const std::initializer_list flagsEnabled = { + // EXAMPLE_FEATURE, B64_WIFI_SCANNING, + SENSOR_CONFIG + // Add enabled flags here + }; - // Add enabled flags here - }; - - static constexpr auto flags = []{ - constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1; - std::array packed{}; + static constexpr auto flags = []{ + constexpr uint32_t flagsLength = EFirmwareFeatureFlags::BITS_TOTAL / 8 + 1; + std::array packed{}; - for (uint32_t bit : flagsEnabled) { - packed[bit / 8] |= 1 << (bit % 8); - } + for (uint32_t bit : flagsEnabled) { + packed[bit / 8] |= 1 << (bit % 8); + } - return packed; - }(); + return packed; + }(); }; #endif diff --git a/src/network/packets.h b/src/network/packets.h index e962a6af0..47978042d 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -47,6 +47,9 @@ #define PACKET_TEMPERATURE 20 // #define PACKET_USER_ACTION 21 // Joycon buttons only currently #define PACKET_FEATURE_FLAGS 22 +// #define PACKET_ROTATION_ACCELERATION 23 // Unification of rot and accel data in one packet +#define PACKET_ACKNOWLEDGE_CONFIG_CHANGE 24 +#define PACKET_SET_CONFIG_FLAG 25 #define PACKET_BUNDLE 100 diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index c682c6557..a6a65dd32 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -110,14 +110,14 @@ void BMI160Sensor::motionSetup() { // Initialize the configuration { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::BMI160: - m_Calibration = sensorCalibration.data.bmi160; + switch (sensorConfig.type) { + case SlimeVR::Configuration::SensorConfigType::BMI160: + m_Config = sensorConfig.data.bmi160; break; - case SlimeVR::Configuration::CalibrationConfigType::NONE: + case SlimeVR::Configuration::SensorConfigType::NONE: m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); break; @@ -161,7 +161,7 @@ void BMI160Sensor::motionSetup() { // allocate temperature memory after calibration because OOM gyroTempCalibrator = new GyroTemperatureCalibrator( - SlimeVR::Configuration::CalibrationConfigType::BMI160, + SlimeVR::Configuration::SensorConfigType::BMI160, sensorId, BMI160_GYRO_TYPICAL_SENSITIVITY_LSB, BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP @@ -171,9 +171,9 @@ void BMI160Sensor::motionSetup() { gyroTempCalibrator->loadConfig(BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); if (gyroTempCalibrator->config.hasCoeffs) { float GOxyzAtTemp[3]; - gyroTempCalibrator->approximateOffset(m_Calibration.temperature, GOxyzAtTemp); + gyroTempCalibrator->approximateOffset(m_Config.temperature, GOxyzAtTemp); for (uint32_t i = 0; i < 3; i++) { - GOxyzStaticTempCompensated[i] = m_Calibration.G_off[i] - GOxyzAtTemp[i]; + GOxyzStaticTempCompensated[i] = m_Config.G_off[i] - GOxyzAtTemp[i]; } } #endif @@ -506,9 +506,9 @@ void BMI160Sensor::onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int1 #endif sensor_real_t gyroCalibratedStatic[3]; - gyroCalibratedStatic[0] = (sensor_real_t)((((double)x - m_Calibration.G_off[0]) * gscaleX)); - gyroCalibratedStatic[1] = (sensor_real_t)((((double)y - m_Calibration.G_off[1]) * gscaleY)); - gyroCalibratedStatic[2] = (sensor_real_t)((((double)z - m_Calibration.G_off[2]) * gscaleZ)); + gyroCalibratedStatic[0] = (sensor_real_t)((((double)x - m_Config.G_off[0]) * gscaleX)); + gyroCalibratedStatic[1] = (sensor_real_t)((((double)y - m_Config.G_off[1]) * gscaleY)); + gyroCalibratedStatic[2] = (sensor_real_t)((((double)z - m_Config.G_off[2]) * gscaleZ)); #if BMI160_USE_TEMPCAL float GOxyz[3]; @@ -629,10 +629,10 @@ void BMI160Sensor::applyAccelCalibrationAndScale(sensor_real_t Axyz[3]) { #if useFullCalibrationMatrix == true float tmp[3]; for (uint8_t i = 0; i < 3; i++) - tmp[i] = (Axyz[i] - m_Calibration.A_B[i]); - Axyz[0] = m_Calibration.A_Ainv[0][0] * tmp[0] + m_Calibration.A_Ainv[0][1] * tmp[1] + m_Calibration.A_Ainv[0][2] * tmp[2]; - Axyz[1] = m_Calibration.A_Ainv[1][0] * tmp[0] + m_Calibration.A_Ainv[1][1] * tmp[1] + m_Calibration.A_Ainv[1][2] * tmp[2]; - Axyz[2] = m_Calibration.A_Ainv[2][0] * tmp[0] + m_Calibration.A_Ainv[2][1] * tmp[1] + m_Calibration.A_Ainv[2][2] * tmp[2]; + tmp[i] = (Axyz[i] - m_Config.A_B[i]); + Axyz[0] = m_Config.A_Ainv[0][0] * tmp[0] + m_Config.A_Ainv[0][1] * tmp[1] + m_Config.A_Ainv[0][2] * tmp[2]; + Axyz[1] = m_Config.A_Ainv[1][0] * tmp[0] + m_Config.A_Ainv[1][1] * tmp[1] + m_Config.A_Ainv[1][2] * tmp[2]; + Axyz[2] = m_Config.A_Ainv[2][0] * tmp[0] + m_Config.A_Ainv[2][1] * tmp[1] + m_Config.A_Ainv[2][2] * tmp[2]; #else for (uint8_t i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - calibration->A_B[i]); @@ -649,20 +649,20 @@ void BMI160Sensor::applyMagCalibrationAndScale(sensor_real_t Mxyz[3]) { #if useFullCalibrationMatrix == true float temp[3]; for (uint8_t i = 0; i < 3; i++) - 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]; + temp[i] = (Mxyz[i] - m_Config.M_B[i]); + Mxyz[0] = m_Config.M_Ainv[0][0] * temp[0] + m_Config.M_Ainv[0][1] * temp[1] + m_Config.M_Ainv[0][2] * temp[2]; + Mxyz[1] = m_Config.M_Ainv[1][0] * temp[0] + m_Config.M_Ainv[1][1] * temp[1] + m_Config.M_Ainv[1][2] * temp[2]; + Mxyz[2] = m_Config.M_Ainv[2][0] * temp[0] + m_Config.M_Ainv[2][1] * temp[1] + m_Config.M_Ainv[2][2] * temp[2]; #else for (i = 0; i < 3; i++) - Mxyz[i] = (Mxyz[i] - m_Calibration.M_B[i]); + Mxyz[i] = (Mxyz[i] - m_Config.M_B[i]); #endif #endif } bool BMI160Sensor::hasGyroCalibration() { for (int i = 0; i < 3; i++) { - if (m_Calibration.G_off[i] != 0.0) + if (m_Config.G_off[i] != 0.0) return true; } return false; @@ -670,10 +670,10 @@ bool BMI160Sensor::hasGyroCalibration() { bool BMI160Sensor::hasAccelCalibration() { for (int i = 0; i < 3; i++) { - if (m_Calibration.A_B[i] != 0.0 || - m_Calibration.A_Ainv[0][i] != 0.0 || - m_Calibration.A_Ainv[1][i] != 0.0 || - m_Calibration.A_Ainv[2][i] != 0.0) + if (m_Config.A_B[i] != 0.0 || + m_Config.A_Ainv[0][i] != 0.0 || + m_Config.A_Ainv[1][i] != 0.0 || + m_Config.A_Ainv[2][i] != 0.0) return true; } return false; @@ -681,10 +681,10 @@ bool BMI160Sensor::hasAccelCalibration() { bool BMI160Sensor::hasMagCalibration() { for (int i = 0; i < 3; i++) { - if (m_Calibration.M_B[i] != 0.0 || - m_Calibration.M_Ainv[0][i] != 0.0 || - m_Calibration.M_Ainv[1][i] != 0.0 || - m_Calibration.M_Ainv[2][i] != 0.0) + if (m_Config.M_B[i] != 0.0 || + m_Config.M_Ainv[0][i] != 0.0 || + m_Config.M_Ainv[1][i] != 0.0 || + m_Config.M_Ainv[2][i] != 0.0) return true; } return false; @@ -699,10 +699,10 @@ void BMI160Sensor::startCalibration(int calibrationType) { 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); + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::BMI160; + config.data.bmi160 = m_Config; + configuration.setSensor(sensorId, config); configuration.save(); m_Logger.debug("Saved the calibration data"); @@ -740,7 +740,7 @@ void BMI160Sensor::maybeCalibrateGyro() { if (!getTemperature(&temperature)) { m_Logger.error("Error: can't read temperature"); } - m_Calibration.temperature = temperature; + m_Config.temperature = temperature; #ifdef DEBUG_SENSOR m_Logger.trace("Calibration temperature: %f", temperature); @@ -768,12 +768,12 @@ void BMI160Sensor::maybeCalibrateGyro() { rawGxyz[2] += gz; } ledManager.off(); - m_Calibration.G_off[0] = ((double)rawGxyz[0]) / gyroCalibrationSamples; - m_Calibration.G_off[1] = ((double)rawGxyz[1]) / gyroCalibrationSamples; - m_Calibration.G_off[2] = ((double)rawGxyz[2]) / gyroCalibrationSamples; + m_Config.G_off[0] = ((double)rawGxyz[0]) / gyroCalibrationSamples; + m_Config.G_off[1] = ((double)rawGxyz[1]) / gyroCalibrationSamples; + m_Config.G_off[2] = ((double)rawGxyz[2]) / gyroCalibrationSamples; #ifdef DEBUG_SENSOR - m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Calibration.G_off)); + m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Config.G_off)); #endif } @@ -904,10 +904,10 @@ void BMI160Sensor::maybeCalibrateAccel() { m_Logger.debug("Accelerometer calibration matrix:"); m_Logger.debug("{"); for (int i = 0; i < 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_Config.A_B[i] = A_BAinv[0][i]; + m_Config.A_Ainv[0][i] = A_BAinv[1][i]; + m_Config.A_Ainv[1][i] = A_BAinv[2][i]; + m_Config.A_Ainv[2][i] = A_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); } m_Logger.debug("}"); @@ -964,10 +964,10 @@ void BMI160Sensor::maybeCalibrateMag() { m_Logger.debug("[INFO] Magnetometer calibration matrix:"); m_Logger.debug("{"); for (int i = 0; i < 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_Config.M_B[i] = M_BAinv[0][i]; + m_Config.M_Ainv[0][i] = M_BAinv[1][i]; + m_Config.M_Ainv[1][i] = M_BAinv[2][i]; + m_Config.M_Ainv[2][i] = M_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); } m_Logger.debug("}"); diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 6d842e9a8..55a8c895b 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -97,7 +97,7 @@ constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min( // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 16.4f // 2000 deg 0 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 32.8f // 1000 deg 1 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 65.6f // 500 deg 2 -// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg 3 +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg 3 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 262.4f // 125 deg 4 constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB = (16.4f * (1 << BMI160_GYRO_RANGE)); @@ -146,7 +146,7 @@ class BMI160Sensor : public Sensor { void maybeCalibrateGyro(); void maybeCalibrateAccel(); void maybeCalibrateMag(); - + void printTemperatureCalibrationState() override final; void printDebugTemperatureCalibrationState() override final; void resetTemperatureCalibrationState() override final { @@ -232,7 +232,7 @@ class BMI160Sensor : public Sensor { bool isAccelCalibrated = false; bool isMagCalibrated = false; - SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration = {}; + SlimeVR::Configuration::BMI160SensorConfig m_Config = {}; }; #endif diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 0035cc85e..19054388b 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -34,7 +34,7 @@ class BNO055Sensor : public Sensor static constexpr auto TypeID = ImuID::BNO055; static constexpr uint8_t Address = 0x28; - BNO055Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + BNO055Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) : Sensor("BNO055Sensor", ImuID::BNO055, id, Address+addrSuppl, rotation, sclPin, sdaPin){}; ~BNO055Sensor(){}; void motionSetup() override final; @@ -43,6 +43,7 @@ class BNO055Sensor : public Sensor private: Adafruit_BNO055 imu; + SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {}; }; #endif diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index bc7594ea0..7699c390b 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -53,23 +53,34 @@ void BNO080Sensor::motionSetup() this->imu.enableLinearAccelerometer(10); -#if USE_6_AXIS - if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { - imu.enableARVRStabilizedGameRotationVector(10); - } else { - imu.enableGameRotationVector(10); - } - - #if BNO_USE_MAGNETOMETER_CORRECTION - imu.enableRotationVector(1000); - #endif -#else - if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { - imu.enableARVRStabilizedRotationVector(10); + SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorConfig.type) { + case SlimeVR::Configuration::SensorConfigType::BNO0XX: + m_Config = sensorConfig.data.bno0XX; + magStatus = m_Config.magEnabled ? MagnetometerStatus::MAG_ENABLED + : MagnetometerStatus::MAG_DISABLED; + break; + default: + // Ignore lack of config for BNO, by default use from FW build + magStatus = USE_6_AXIS ? MagnetometerStatus::MAG_DISABLED + : MagnetometerStatus::MAG_ENABLED; + break; + } + + if(!isMagEnabled()) { + if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedGameRotationVector(10); + } else { + imu.enableGameRotationVector(10); + } } else { - imu.enableRotationVector(10); + if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { + imu.enableARVRStabilizedRotationVector(10); + } else { + imu.enableRotationVector(10); + } } -#endif #if ENABLE_INSPECTION imu.enableRawGyro(10); @@ -113,52 +124,37 @@ void BNO080Sensor::motionLoop() lastReset = 0; lastData = millis(); -#if USE_6_AXIS - if (imu.hasNewGameQuat()) // New quaternion if context - { - Quat nRotation; - imu.getGameQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, calibrationAccuracy); + if(!isMagEnabled()) { + if (imu.hasNewGameQuat()) // New quaternion if context + { + Quat nRotation; + imu.getGameQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, calibrationAccuracy); - setFusedRotation(nRotation); - // Leave new quaternion if context open, it's closed later + setFusedRotation(nRotation); + // Leave new quaternion if context open, it's closed later + } + } else { -#else // USE_6_AXIS + if (imu.hasNewQuat()) // New quaternion if context + { + Quat nRotation; + imu.getQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, magneticAccuracyEstimate, calibrationAccuracy); - if (imu.hasNewQuat()) // New quaternion if context - { - Quat nRotation; - imu.getQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, magneticAccuracyEstimate, calibrationAccuracy); + setFusedRotation(nRotation); - setFusedRotation(nRotation); - // Leave new quaternion if context open, it's closed later -#endif // USE_6_AXIS + // Leave new quaternion if context open, it's closed later + } // Closing new quaternion if context + } - // Continuation of the new quaternion if context, used for both 6 and 9 axis + // Continuation of the new quaternion if context, used for both 6 and 9 axis #if SEND_ACCELERATION - { - uint8_t acc; - Vector3 nAccel; - imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc); - setAcceleration(nAccel); - } -#endif // SEND_ACCELERATION - } // Closing new quaternion if context - -#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION - if (imu.hasNewMagQuat()) { - imu.getMagQuat(magQuaternion.x, magQuaternion.y, magQuaternion.z, magQuaternion.w, magneticAccuracyEstimate, magCalibrationAccuracy); - magQuaternion *= sensorOffset; - - #if ENABLE_INSPECTION - { - networkConnection.sendInspectionCorrectionData(sensorId, quaternion); - } - #endif // ENABLE_INSPECTION - - newMagData = true; + uint8_t acc; + Vector3 nAccel; + imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc); + setAcceleration(nAccel); } -#endif // USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION +#endif // SEND_ACCELERATION if (imu.getTapDetected()) { @@ -193,7 +189,8 @@ void BNO080Sensor::motionLoop() } } -SensorStatus BNO080Sensor::getSensorState() { +SensorStatus BNO080Sensor::getSensorState() +{ return lastReset > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } @@ -217,19 +214,6 @@ void BNO080Sensor::sendData() #endif } -#if !USE_6_AXIS - networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); -#endif - -#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION - if (newMagData) - { - newMagData = false; - networkConnection.sendRotationData(sensorId, &magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy); - networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); - } -#endif - if (tap != 0) { networkConnection.sendSensorTap(sensorId, tap); @@ -237,6 +221,23 @@ void BNO080Sensor::sendData() } } +void BNO080Sensor::setFlag(uint16_t flagId, bool state) +{ + if(flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) { + m_Config.magEnabled = state; + magStatus = state ? MagnetometerStatus::MAG_ENABLED + : MagnetometerStatus::MAG_DISABLED; + + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::BNO0XX; + config.data.bno0XX = m_Config; + configuration.setSensor(sensorId, config); + + // Reinitialize the sensor + motionSetup(); + } +} + void BNO080Sensor::startCalibration(int calibrationType) { // BNO does automatic calibration, diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 6056121aa..3f04a7507 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -27,6 +27,8 @@ #include "sensor.h" #include +#define FLAG_SENSOR_BNO0XX_MAG_ENABLED 1 + class BNO080Sensor : public Sensor { public: @@ -45,6 +47,7 @@ class BNO080Sensor : public Sensor void sendData() override final; void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; + void setFlag(uint16_t flagId, bool state) override final; protected: // forwarding constructor @@ -59,6 +62,7 @@ class BNO080Sensor : public Sensor unsigned long lastData = 0; uint8_t lastReset = 0; BNO080Error lastError{}; + SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {}; // Magnetometer specific members Quat magQuaternion{}; diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index d43af39f1..c3159a268 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -393,32 +393,32 @@ void ICM20948Sensor::saveCalibration(bool repeat) 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.GetBiasGyroX(&m_Config.G[0]); + imu.GetBiasGyroY(&m_Config.G[1]); + imu.GetBiasGyroZ(&m_Config.G[2]); - imu.GetBiasAccelX(&m_Calibration.A[0]); - imu.GetBiasAccelY(&m_Calibration.A[1]); - imu.GetBiasAccelZ(&m_Calibration.A[2]); + imu.GetBiasAccelX(&m_Config.A[0]); + imu.GetBiasAccelY(&m_Config.A[1]); + imu.GetBiasAccelZ(&m_Config.A[2]); #if !USE_6_AXIS - imu.GetBiasCPassX(&m_Calibration.C[0]); - imu.GetBiasCPassY(&m_Calibration.C[1]); - imu.GetBiasCPassZ(&m_Calibration.C[2]); + imu.GetBiasCPassX(&m_Config.C[0]); + imu.GetBiasCPassY(&m_Config.C[1]); + imu.GetBiasCPassZ(&m_Config.C[2]); #endif #ifdef DEBUG_SENSOR - m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.G)); - m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.A)); + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.A)); #if !USE_6_AXIS - m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.C)); #endif #endif - SlimeVR::Configuration::CalibrationConfig calibration; - calibration.type = SlimeVR::Configuration::CalibrationConfigType::ICM20948; - calibration.data.icm20948 = m_Calibration; - configuration.setCalibration(sensorId, calibration); + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::ICM20948; + config.data.icm20948 = m_Config; + configuration.setSensor(sensorId, config); configuration.save(); if (repeat) { @@ -444,14 +444,14 @@ void ICM20948Sensor::loadCalibration() } #endif - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + SlimeVR::Configuration::SensorConfig sensorCalibration = configuration.getSensor(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::ICM20948: - m_Calibration = sensorCalibration.data.icm20948; + case SlimeVR::Configuration::SensorConfigType::ICM20948: + m_Config = sensorCalibration.data.icm20948; break; - case SlimeVR::Configuration::CalibrationConfigType::NONE: + case SlimeVR::Configuration::SensorConfigType::NONE: m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); break; @@ -462,25 +462,25 @@ void ICM20948Sensor::loadCalibration() } #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)); + m_Logger.trace("Gyrometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.G)); + m_Logger.trace("Accelerometer bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.A)); #if !USE_6_AXIS - m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Calibration.C)); + m_Logger.trace("Compass bias: [%d, %d, %d]", UNPACK_VECTOR_ARRAY(m_Config.C)); #endif #endif - imu.SetBiasGyroX(m_Calibration.G[0]); - imu.SetBiasGyroY(m_Calibration.G[1]); - imu.SetBiasGyroZ(m_Calibration.G[2]); + imu.SetBiasGyroX(m_Config.G[0]); + imu.SetBiasGyroY(m_Config.G[1]); + imu.SetBiasGyroZ(m_Config.G[2]); - imu.SetBiasAccelX(m_Calibration.A[0]); - imu.SetBiasAccelY(m_Calibration.A[1]); - imu.SetBiasAccelZ(m_Calibration.A[2]); + imu.SetBiasAccelX(m_Config.A[0]); + imu.SetBiasAccelY(m_Config.A[1]); + imu.SetBiasAccelZ(m_Config.A[2]); #if !USE_6_AXIS - imu.SetBiasCPassX(m_Calibration.C[0]); - imu.SetBiasCPassY(m_Calibration.C[1]); - imu.SetBiasCPassZ(m_Calibration.C[2]); + imu.SetBiasCPassX(m_Config.C[0]); + imu.SetBiasCPassY(m_Config.C[1]); + imu.SetBiasCPassZ(m_Config.C[2]); #endif } diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 2f213e492..d5f4f57c4 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -64,7 +64,7 @@ class ICM20948Sensor : public Sensor icm_20948_DMP_data_t dmpData{}; icm_20948_DMP_data_t dmpDataTemp{}; - SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration = {}; + SlimeVR::Configuration::ICM20948SensorConfig m_Config = {}; SlimeVR::Sensors::SensorFusionDMP sfusion; diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index 44ac31591..f7d49cce8 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -53,14 +53,14 @@ void MPU6050Sensor::motionSetup() #ifndef IMU_MPU6050_RUNTIME_CALIBRATION // Initialize the configuration { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + SlimeVR::Configuration::SensorConfig sensorCalibration = configuration.getCalibration(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::MPU6050: - m_Calibration = sensorCalibration.data.mpu6050; + case SlimeVR::Configuration::SensorConfigType::MPU6050: + m_Config = sensorCalibration.data.mpu6050; break; - case SlimeVR::Configuration::CalibrationConfigType::NONE: + case SlimeVR::Configuration::SensorConfigType::NONE: m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); break; @@ -177,21 +177,21 @@ void MPU6050Sensor::startCalibration(int calibrationType) { { case CALIBRATION_TYPE_INTERNAL_ACCEL: imu.CalibrateAccel(10); - m_Calibration.A_B[0] = imu.getXAccelOffset(); - m_Calibration.A_B[1] = imu.getYAccelOffset(); - m_Calibration.A_B[2] = imu.getZAccelOffset(); + m_Config.A_B[0] = imu.getXAccelOffset(); + m_Config.A_B[1] = imu.getYAccelOffset(); + m_Config.A_B[2] = imu.getZAccelOffset(); break; case CALIBRATION_TYPE_INTERNAL_GYRO: imu.CalibrateGyro(10); - m_Calibration.G_off[0] = imu.getXGyroOffset(); - m_Calibration.G_off[1] = imu.getYGyroOffset(); - m_Calibration.G_off[2] = imu.getZGyroOffset(); + m_Config.G_off[0] = imu.getXGyroOffset(); + m_Config.G_off[1] = imu.getYGyroOffset(); + m_Config.G_off[2] = imu.getZGyroOffset(); break; } - SlimeVR::Configuration::CalibrationConfig calibration; - calibration.type = SlimeVR::Configuration::CalibrationConfigType::MPU6050; - calibration.data.mpu6050 = m_Calibration; + SlimeVR::Configuration::SensorConfig calibration; + calibration.type = SlimeVR::Configuration::SensorConfigType::MPU6050; + calibration.data.mpu6050 = m_Config; configuration.setCalibration(sensorId, calibration); configuration.save(); diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index c5667798f..926bd24b7 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -56,7 +56,7 @@ class MPU6050Sensor : public Sensor SlimeVR::Sensors::SensorFusionDMP sfusion; #ifndef IMU_MPU6050_RUNTIME_CALIBRATION - SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration = {}; + SlimeVR::Configuration::MPU6050SensorConfig m_Config = {}; #endif }; diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index a4295975f..84a7da63b 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -71,14 +71,14 @@ void MPU9250Sensor::motionSetup() { // Initialize the configuration { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::MPU9250: - m_Calibration = sensorCalibration.data.mpu9250; + switch (sensorConfig.type) { + case SlimeVR::Configuration::SensorConfigType::MPU9250: + m_Config = sensorConfig.data.mpu9250; break; - case SlimeVR::Configuration::CalibrationConfigType::NONE: + case SlimeVR::Configuration::SensorConfigType::NONE: m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); break; @@ -224,10 +224,10 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.debug("[INFO] Magnetometer calibration matrix:"); m_Logger.debug("{"); for (int i = 0; i < 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_Config.M_B[i] = M_BAinv[0][i]; + m_Config.M_Ainv[0][i] = M_BAinv[1][i]; + m_Config.M_Ainv[1][i] = M_BAinv[2][i]; + m_Config.M_Ainv[2][i] = M_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); } m_Logger.debug("}"); @@ -265,9 +265,9 @@ void MPU9250Sensor::startCalibration(int calibrationType) { #endif // TODO: use offset registers? - m_Calibration.G_off[0] = Gxyz[0]; - m_Calibration.G_off[1] = Gxyz[1]; - m_Calibration.G_off[2] = Gxyz[2]; + m_Config.G_off[0] = Gxyz[0]; + m_Config.G_off[1] = Gxyz[1]; + m_Config.G_off[2] = Gxyz[2]; // Blink calibrating led before user should rotate the sensor m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data"); @@ -302,20 +302,20 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.debug("{"); for (int i = 0; i < 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_Config.A_B[i] = A_BAinv[0][i]; + m_Config.A_Ainv[0][i] = A_BAinv[1][i]; + m_Config.A_Ainv[1][i] = A_BAinv[2][i]; + m_Config.A_Ainv[2][i] = A_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); } m_Logger.debug("}"); m_Logger.debug("[INFO] Magnetometer calibration matrix:"); m_Logger.debug("{"); for (int i = 0; i < 3; i++) { - m_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_Config.M_B[i] = M_BAinv[0][i]; + m_Config.M_Ainv[0][i] = M_BAinv[1][i]; + m_Config.M_Ainv[1][i] = M_BAinv[2][i]; + m_Config.M_Ainv[2][i] = M_BAinv[3][i]; m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); } m_Logger.debug("}"); @@ -323,10 +323,10 @@ void MPU9250Sensor::startCalibration(int calibrationType) { 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); + SlimeVR::Configuration::SensorConfig config; + config.type = SlimeVR::Configuration::SensorConfigType::MPU9250; + config.data.mpu9250 = m_Config; + configuration.setSensor(sensorId, config); configuration.save(); ledManager.off(); @@ -347,12 +347,12 @@ void MPU9250Sensor::parseMagData(int16_t data[3]) { //apply offsets and scale factors from Magneto for (unsigned i = 0; i < 3; i++) { - temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + temp[i] = (Mxyz[i] - m_Config.M_B[i]); } - + for (unsigned i = 0; i < 3; i++) { #if useFullCalibrationMatrix == true - Mxyz[i] = m_Calibration.M_Ainv[i][0] * temp[0] + m_Calibration.M_Ainv[i][1] * temp[1] + m_Calibration.M_Ainv[i][2] * temp[2]; + Mxyz[i] = m_Config.M_Ainv[i][0] * temp[0] + m_Config.M_Ainv[i][1] * temp[1] + m_Config.M_Ainv[i][2] * temp[2]; #else Mxyz[i] = temp[i]; #endif @@ -372,12 +372,12 @@ void MPU9250Sensor::parseAccelData(int16_t data[3]) { //apply offsets (bias) and scale factors from Magneto for (unsigned i = 0; i < 3; i++) { #if !MPU_USE_DMPMAG - temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + temp[i] = (Axyz[i] - m_Config.A_B[i]); } - + for (unsigned i = 0; i < 3; i++) { #if useFullCalibrationMatrix == true - Axyz[i] = m_Calibration.A_Ainv[i][0] * temp[0] + m_Calibration.A_Ainv[i][1] * temp[1] + m_Calibration.A_Ainv[i][2] * temp[2]; + Axyz[i] = m_Config.A_Ainv[i][0] * temp[0] + m_Config.A_Ainv[i][1] * temp[1] + m_Config.A_Ainv[i][2] * temp[2]; #else Axyz[i] = temp[i]; #endif @@ -389,9 +389,9 @@ void MPU9250Sensor::parseAccelData(int16_t data[3]) { // TODO: refactor so that calibration/conversion to float is only done in one place. void MPU9250Sensor::parseGyroData(int16_t data[3]) { // reading big endian int16 - Gxyz[0] = ((float)data[0] - m_Calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s - Gxyz[1] = ((float)data[1] - m_Calibration.G_off[1]) * gscale; - Gxyz[2] = ((float)data[2] - m_Calibration.G_off[2]) * gscale; + Gxyz[0] = ((float)data[0] - m_Config.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s + Gxyz[1] = ((float)data[1] - m_Config.G_off[1]) * gscale; + Gxyz[2] = ((float)data[2] - m_Config.G_off[2]) * gscale; } // really just an implementation detail of getNextSample... diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index 25140cb90..02b28a3cf 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -79,7 +79,7 @@ class MPU9250Sensor : public Sensor VectorInt16 rawAccel{}; Quat correction{0, 0, 0, 0}; - SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration = {}; + SlimeVR::Configuration::MPU9250SensorConfig m_Config = {}; // outputs to respective member variables void parseAccelData(int16_t data[3]); diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 94299f12c..f5aa53f04 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -69,6 +69,11 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +uint16_t Sensor::getSensorConfigData() { + SlimeVR::Configuration::SensorConfig sensorConfig = configuration.getSensor(sensorId); + return SlimeVR::Configuration::configDataToNumber(sensorConfig); +} + const char * getIMUNameByType(ImuID imuType) { switch(imuType) { case ImuID::MPU9250: diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index f225aebd3..d678ba400 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -1,24 +1,24 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_SENSOR_H_ @@ -36,85 +36,100 @@ #define DATA_TYPE_CORRECTION 2 enum class SensorStatus : uint8_t { - SENSOR_OFFLINE = 0, - SENSOR_OK = 1, - SENSOR_ERROR = 2 + SENSOR_OFFLINE = 0, + SENSOR_OK = 1, + SENSOR_ERROR = 2 +}; + +enum class MagnetometerStatus : uint8_t { + MAG_NOT_SUPPORTED = 0, + MAG_DISABLED = 1, + MAG_ENABLED = 2, }; class Sensor { public: - Sensor(const char *sensorName, ImuID type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0) - : addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)), - sclPin(sclpin), sdaPin(sdapin) - { - char buf[4]; - sprintf(buf, "%u", id); - m_Logger.setTag(buf); - } - - virtual ~Sensor(){}; - virtual void motionSetup(){}; - virtual void postSetup(){}; - virtual void motionLoop(){}; - virtual void sendData(); - virtual void setAcceleration(Vector3 a); - virtual void setFusedRotation(Quat r); - virtual void startCalibration(int calibrationType){}; - virtual SensorStatus getSensorState(); - virtual void printTemperatureCalibrationState(); - virtual void printDebugTemperatureCalibrationState(); - virtual void resetTemperatureCalibrationState(); - virtual void saveTemperatureCalibration(); - bool isWorking() { - return working; - }; - bool getHadData() const { - return hadData; - }; - bool isValid() { - return sclPin != sdaPin; - }; - uint8_t getSensorId() { - return sensorId; - }; - ImuID getSensorType() { - return sensorType; - }; - const Vector3& getAcceleration() { - return acceleration; - }; - const Quat& getFusedRotation() { - return fusedRotation; - }; - bool hasNewDataToSend() { - return newFusedRotation || newAcceleration; - }; + Sensor(const char *sensorName, ImuID type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0) + : addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)), + sclPin(sclpin), sdaPin(sdapin) + { + char buf[4]; + sprintf(buf, "%u", id); + m_Logger.setTag(buf); + } + + virtual ~Sensor(){}; + virtual void motionSetup(){}; + virtual void postSetup(){}; + virtual void motionLoop(){}; + virtual void sendData(); + virtual void setAcceleration(Vector3 a); + virtual void setFusedRotation(Quat r); + virtual void startCalibration(int calibrationType){}; + virtual SensorStatus getSensorState(); + virtual void printTemperatureCalibrationState(); + virtual void printDebugTemperatureCalibrationState(); + virtual void resetTemperatureCalibrationState(); + virtual void saveTemperatureCalibration(); + virtual void setFlag(uint16_t flagId, bool state){}; + virtual uint16_t getSensorConfigData(); + bool isWorking() { + return working; + }; + bool getHadData() const { + return hadData; + }; + bool isValid() { + return sclPin != sdaPin; + }; + bool isMagEnabled() { + return magStatus == MagnetometerStatus::MAG_ENABLED; + }; + uint8_t getSensorId() { + return sensorId; + }; + ImuID getSensorType() { + return sensorType; + }; + MagnetometerStatus getMagStatus() { + return magStatus; + }; + const Vector3& getAcceleration() { + return acceleration; + }; + const Quat& getFusedRotation() { + return fusedRotation; + }; + bool hasNewDataToSend() { + return newFusedRotation || newAcceleration; + }; protected: - uint8_t addr = 0; - uint8_t sensorId = 0; - ImuID sensorType = ImuID::Unknown; - bool working = false; - bool hadData = false; - uint8_t calibrationAccuracy = 0; - Quat sensorOffset; - - bool newFusedRotation = false; - Quat fusedRotation{}; - Quat lastFusedRotationSent{}; - - bool newAcceleration = false; - Vector3 acceleration{}; - - mutable SlimeVR::Logging::Logger m_Logger; - + uint8_t addr = 0; + uint8_t sensorId = 0; + ImuID sensorType = ImuID::Unknown; + bool working = false; + bool hadData = false; + uint8_t calibrationAccuracy = 0; + MagnetometerStatus magStatus = MagnetometerStatus::MAG_NOT_SUPPORTED; + Quat sensorOffset; + + bool newFusedRotation = false; + Quat fusedRotation{}; + Quat lastFusedRotationSent{}; + + bool newAcceleration = false; + Vector3 acceleration{}; + + mutable SlimeVR::Logging::Logger m_Logger; + public: - uint8_t sclPin = 0; - uint8_t sdaPin = 0; + uint8_t sclPin = 0; + uint8_t sdaPin = 0; private: - void printTemperatureCalibrationUnsupported(); + void printTemperatureCalibrationUnsupported(); }; const char * getIMUNameByType(ImuID imuType); diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 5f70e5ff0..a9dacbe4e 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -211,16 +211,16 @@ class SoftFusionSensor : public Sensor return; } - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + SlimeVR::Configuration::SensorConfig sensorCalibration = configuration.getSensor(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out - if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::SFUSION + if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::SFUSION && (sensorCalibration.data.sfusion.ImuType == imu::Type) && (sensorCalibration.data.sfusion.MotionlessDataLen == MotionlessCalibDataSize())) { m_calibration = sensorCalibration.data.sfusion; recalcFusion(); } - else if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::NONE) { + else if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::NONE) { m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); } @@ -230,7 +230,7 @@ class SoftFusionSensor : public Sensor } bool initResult = false; - + if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData)); @@ -311,10 +311,10 @@ class SoftFusionSensor : public Sensor void saveCalibration() { m_Logger.debug("Saving the calibration data"); - SlimeVR::Configuration::CalibrationConfig calibration; - calibration.type = SlimeVR::Configuration::CalibrationConfigType::SFUSION; + SlimeVR::Configuration::SensorConfig calibration; + calibration.type = SlimeVR::Configuration::SensorConfigType::SFUSION; calibration.data.sfusion = m_calibration; - configuration.setCalibration(sensorId, calibration); + configuration.setSensor(sensorId, calibration); configuration.save(); } @@ -507,7 +507,7 @@ class SoftFusionSensor : public Sensor SensorFusionRestDetect m_fusion; T m_sensor; - SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration = { + SlimeVR::Configuration::SoftFusionSensorConfig m_calibration = { // let's create here transparent calibration that doesn't affect input data .ImuType = {imu::Type}, .MotionlessDataLen = {MotionlessCalibDataSize()},