From 84c2b4e0dd4a237048b0053260c38f8a96979a5b Mon Sep 17 00:00:00 2001 From: TheDevMinerTV Date: Sun, 6 Mar 2022 17:05:18 +0100 Subject: [PATCH] Rewrite the SensorFactory --- src/debug.h | 2 - src/main.cpp | 29 +--- src/network/udpclient.cpp | 93 ++++++++---- .../{sensorfactory.h => EmptySensor.h} | 40 ++--- src/sensors/ErroneousSensor.cpp | 41 ++++++ src/sensors/ErroneousSensor.h | 51 +++++++ src/sensors/SensorManager.cpp | 139 ++++++++++++++++++ src/sensors/SensorManager.h | 62 ++++++++ src/sensors/bmi160sensor.h | 2 +- src/sensors/bno055sensor.h | 2 +- src/sensors/bno080sensor.cpp | 6 +- src/sensors/bno080sensor.h | 5 +- src/sensors/icm20948sensor.h | 19 +-- src/sensors/mpu6050sensor.h | 4 +- src/sensors/mpu9250sensor.h | 12 +- src/sensors/sensor.cpp | 12 -- src/sensors/sensor.h | 25 ++-- src/sensors/sensorfactory.cpp | 136 ----------------- 18 files changed, 422 insertions(+), 258 deletions(-) rename src/sensors/{sensorfactory.h => EmptySensor.h} (67%) create mode 100644 src/sensors/ErroneousSensor.cpp create mode 100644 src/sensors/ErroneousSensor.h create mode 100644 src/sensors/SensorManager.cpp create mode 100644 src/sensors/SensorManager.h delete mode 100644 src/sensors/sensorfactory.cpp diff --git a/src/debug.h b/src/debug.h index 68f713f..6a8c40b 100644 --- a/src/debug.h +++ b/src/debug.h @@ -43,8 +43,6 @@ #define serialDebug false // Set to true to get Serial output for debugging #define serialBaudRate 115200 -#define UPDATE_IMU_UNCONNECTED 1 -//#define SEND_UPDATES_UNCONNECTED 1 #define LED_INTERVAL_STANDBUY 10000 #define STATUS_PRINT_INTERVAL 15000 #define ENABLE_LEDS true diff --git a/src/main.cpp b/src/main.cpp index cb9888d..f9aefd5 100644 --- a/src/main.cpp +++ b/src/main.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 @@ -23,7 +23,7 @@ #include "Wire.h" #include "ota.h" -#include "sensors/sensorfactory.h" +#include "sensors/SensorManager.h" #include "configuration.h" #include "network/network.h" #include "globals.h" @@ -35,8 +35,8 @@ #include "logging/Logger.h" SlimeVR::Logging::Logger logger("SlimeVR"); +SlimeVR::Sensors::SensorManager sensorManager; -SensorFactory sensors {}; int sensorToCalibrate = -1; bool blinking = false; unsigned long blinkStart = 0; @@ -79,8 +79,7 @@ void setup() // Wait for IMU to boot delay(500); - sensors.create(); - sensors.motionSetup(); + sensorManager.setup(); Network::setUp(); OTA::otaSetup(otaPassword); @@ -94,24 +93,8 @@ void loop() LEDManager::ledStatusUpdate(); SerialCommands::update(); OTA::otaUpdate(); - Network::update(sensors.getFirst(), sensors.getSecond()); -#ifndef UPDATE_IMU_UNCONNECTED - if (ServerConnection::isConnected()) - { -#endif - sensors.motionLoop(); -#ifndef UPDATE_IMU_UNCONNECTED - } -#endif - // Send updates -#ifndef SEND_UPDATES_UNCONNECTED - if (ServerConnection::isConnected()) - { -#endif - sensors.sendData(); -#ifndef SEND_UPDATES_UNCONNECTED - } -#endif + Network::update(sensorManager.getFirst(), sensorManager.getSecond()); + sensorManager.update(); battery.Loop(); #ifdef TARGET_LOOPTIME_MICROS diff --git a/src/network/udpclient.cpp b/src/network/udpclient.cpp index 2bda4c6..78a3984 100644 --- a/src/network/udpclient.cpp +++ b/src/network/udpclient.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 @@ -152,6 +152,11 @@ namespace DataTransfer { // PACKET_HEARTBEAT 0 void Network::sendHeartbeat() { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_HEARTBEAT); DataTransfer::sendPacketNumber(); @@ -161,9 +166,11 @@ void Network::sendHeartbeat() { // PACKET_ACCEL 4 void Network::sendAccel(float* vector, uint8_t sensorId) { - #ifndef SEND_UPDATES_UNCONNECTED - if(!connected) return; // bno080sensor.cpp function call not in sendData() but in motionLoop() - #endif + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_ACCEL); DataTransfer::sendPacketNumber(); @@ -176,9 +183,11 @@ void Network::sendAccel(float* vector, uint8_t sensorId) { // PACKET_RAW_CALIBRATION_DATA 6 void Network::sendRawCalibrationData(float* vector, uint8_t calibrationType, uint8_t sensorId) { - #ifndef SEND_UPDATES_UNCONNECTED - if(!connected) return; // mpu9250sensor.cpp startCalibration() - #endif + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA); DataTransfer::sendPacketNumber(); @@ -192,9 +201,11 @@ void Network::sendRawCalibrationData(float* vector, uint8_t calibrationType, uin } void Network::sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8_t sensorId) { - #ifndef SEND_UPDATES_UNCONNECTED - if(!connected) return; // function not used? - #endif + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA); DataTransfer::sendPacketNumber(); @@ -209,9 +220,11 @@ void Network::sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8 // PACKET_CALIBRATION_FINISHED 7 void Network::sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId) { - #ifndef SEND_UPDATES_UNCONNECTED - if(!connected) return; // mpu6050sensor.cpp mpu9250sensor.cpp startCalibration() - #endif + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_CALIBRATION_FINISHED); DataTransfer::sendPacketNumber(); @@ -223,9 +236,11 @@ void Network::sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId) // PACKET_BATTERY_LEVEL 12 void Network::sendBatteryLevel(float batteryVoltage, float batteryPercentage) { - #ifndef SEND_UPDATES_UNCONNECTED - if(!connected) return; - #endif + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_BATTERY_LEVEL); DataTransfer::sendPacketNumber(); @@ -237,6 +252,11 @@ void Network::sendBatteryLevel(float batteryVoltage, float batteryPercentage) { // PACKET_TAP 13 void Network::sendTap(uint8_t value, uint8_t sensorId) { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_TAP); DataTransfer::sendPacketNumber(); @@ -248,9 +268,11 @@ void Network::sendTap(uint8_t value, uint8_t sensorId) { // PACKET_ERROR 14 void Network::sendError(uint8_t reason, uint8_t sensorId) { - #ifndef SEND_UPDATES_UNCONNECTED - if(!connected) return; - #endif + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_ERROR); DataTransfer::sendPacketNumber(); @@ -262,6 +284,11 @@ void Network::sendError(uint8_t reason, uint8_t sensorId) { // PACKET_SENSOR_INFO 15 void Network::sendSensorInfo(Sensor * sensor) { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_SENSOR_INFO); DataTransfer::sendPacketNumber(); @@ -274,6 +301,11 @@ void Network::sendSensorInfo(Sensor * sensor) { // PACKET_ROTATION_DATA 17 void Network::sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_t accuracyInfo, uint8_t sensorId) { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_ROTATION_DATA); DataTransfer::sendPacketNumber(); @@ -290,6 +322,11 @@ void Network::sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_ // PACKET_MAGNETOMETER_ACCURACY 18 void Network::sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId) { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_MAGNETOMETER_ACCURACY); DataTransfer::sendPacketNumber(); @@ -301,6 +338,11 @@ void Network::sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId) { // PACKET_SIGNAL_STRENGTH 19 void Network::sendSignalStrength(uint8_t signalStrength) { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_SIGNAL_STRENGTH); DataTransfer::sendPacketNumber(); @@ -312,6 +354,11 @@ void Network::sendSignalStrength(uint8_t signalStrength) { // PACKET_TEMPERATURE 20 void Network::sendTemperature(float temperature, uint8_t sensorId) { + if(!connected) + { + return; + } + if(DataTransfer::beginPacket()) { DataTransfer::sendPacketType(PACKET_TEMPERATURE); DataTransfer::sendPacketNumber(); @@ -545,9 +592,7 @@ void ServerConnection::connect() lastPacketMs = now; connected = true; LEDManager::unsetLedStatus(LED_STATUS_SERVER_CONNECTING); -#ifndef SEND_UPDATES_UNCONNECTED LEDManager::off(LOADING_LED); -#endif udpClientLogger.debug("Handshake successful, server is %s:%d", Udp.remoteIP().toString().c_str(), + Udp.remotePort()); return; default: @@ -557,23 +602,19 @@ void ServerConnection::connect() else { break; - } + } } if(lastConnectionAttemptMs + 1000 < now) { lastConnectionAttemptMs = now; udpClientLogger.info("Looking for the server..."); Network::sendHandshake(); -#ifndef SEND_UPDATES_UNCONNECTED LEDManager::on(LOADING_LED); -#endif } -#ifndef SEND_UPDATES_UNCONNECTED else if(lastConnectionAttemptMs + 20 < now) { LEDManager::off(LOADING_LED); } -#endif } void ServerConnection::resetConnection() { diff --git a/src/sensors/sensorfactory.h b/src/sensors/EmptySensor.h similarity index 67% rename from src/sensors/sensorfactory.h rename to src/sensors/EmptySensor.h index bbc38d9..dff2693 100644 --- a/src/sensors/sensorfactory.h +++ b/src/sensors/EmptySensor.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + 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 @@ -20,28 +20,28 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#ifndef SLIMEVR_SENSORFACTORY_H_ -#define SLIMEVR_SENSORFACTORY_H_ -#include "globals.h" +#ifndef SENSORS_EMPTYSENSOR_H +#define SENSORS_EMPTYSENSOR_H + #include "sensor.h" -class SensorFactory +namespace SlimeVR { -public: - SensorFactory(); - ~SensorFactory(); - void create(); - void motionSetup(); - void motionLoop(); - void sendData(); - void startCalibration(int sensorId, int calibrationType); - Sensor *getFirst() { return sensor1; }; - Sensor *getSecond() { return sensor2; }; + namespace Sensors + { + class EmptySensor : public Sensor + { + public: + EmptySensor(uint8_t id) : Sensor("EmptySensor", 255, id, 0, 0.0){}; + ~EmptySensor(){}; -protected: - Sensor *sensor1; - Sensor *sensor2; -}; + void motionSetup() override final{}; + void motionLoop() override final{}; + void sendData() override final{}; + void startCalibration(int calibrationType) override final{}; + }; + } +} -#endif // SLIMEVR_SENSORFACTORY_H_ \ No newline at end of file +#endif diff --git a/src/sensors/ErroneousSensor.cpp b/src/sensors/ErroneousSensor.cpp new file mode 100644 index 0000000..340b7d8 --- /dev/null +++ b/src/sensors/ErroneousSensor.cpp @@ -0,0 +1,41 @@ +/* + 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 "ErroneousSensor.h" +#include "network/network.h" + +namespace SlimeVR +{ + namespace Sensors + { + void ErroneousSensor::motionSetup() + { + m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType)); + } + + uint8_t ErroneousSensor::getSensorState() + { + return SENSOR_ERROR; + }; + } +} diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h new file mode 100644 index 0000000..8e2e2e7 --- /dev/null +++ b/src/sensors/ErroneousSensor.h @@ -0,0 +1,51 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SENSORS_ERRONEOUSSENSOR_H +#define SENSORS_ERRONEOUSSENSOR_H + +#include "sensor.h" + +namespace SlimeVR +{ + namespace Sensors + { + class ErroneousSensor : public Sensor + { + public: + ErroneousSensor(uint8_t id, uint8_t type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){}; + ~ErroneousSensor(){}; + + void motionSetup() override; + void motionLoop() override final{}; + void sendData() override{}; + void startCalibration(int calibrationType) override final{}; + uint8_t getSensorState() override; + + private: + uint8_t m_ExpectedType; + }; + } +} + +#endif diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp new file mode 100644 index 0000000..c8545b2 --- /dev/null +++ b/src/sensors/SensorManager.cpp @@ -0,0 +1,139 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "SensorManager.h" +#include +#include "network/network.h" +#include "bno055sensor.h" +#include "bno080sensor.h" +#include "mpu9250sensor.h" +#include "mpu6050sensor.h" +#include "bmi160sensor.h" +#include "icm20948sensor.h" +#include "ErroneousSensor.h" + +namespace SlimeVR +{ + namespace Sensors + { + void SensorManager::setup() + { + uint8_t firstIMUAddress = 0; + uint8_t secondIMUAddress = 0; + + { +#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 + firstIMUAddress = I2CSCAN::pickDevice(0x4A, 0x4B, true); +#elif IMU == IMU_BNO055 + firstIMUAddress = I2CSCAN::pickDevice(0x29, 0x28, true); +#elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 + firstIMUAddress = I2CSCAN::pickDevice(0x68, 0x69, true); +#else +#error Unsupported primary IMU +#endif + + if (firstIMUAddress == 0) + { + m_Sensor1 = new ErroneousSensor(0, IMU); + } + else + { + m_Logger.trace("Primary IMU found at address 0x%02X", firstIMUAddress); + +#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 + m_Sensor1 = new BNO080Sensor(0, IMU, firstIMUAddress, IMU_ROTATION, PIN_IMU_INT); +#elif IMU == IMU_BNO055 + m_Sensor1 = new BNO055Sensor(0, firstIMUAddress, IMU_ROTATION); +#elif IMU == IMU_MPU9250 + m_Sensor1 = new MPU9250Sensor(0, firstIMUAddress, IMU_ROTATION); +#elif IMU == IMU_BMI160 + m_Sensor1 = new BMI160Sensor(0, firstIMUAddress, IMU_ROTATION); +#elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050 + m_Sensor1 = new MPU6050Sensor(0, IMU, firstIMUAddress, IMU_ROTATION); +#elif IMU == IMU_ICM20948 + m_Sensor1 = new ICM20948Sensor(0, firstIMUAddress, IMU_ROTATION); +#endif + } + + m_Sensor1->motionSetup(); + } + + { +#if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 + secondIMUAddress = I2CSCAN::pickDevice(0x4B, 0x4A, false); +#elif SECOND_IMU == IMU_BNO055 + secondIMUAddress = I2CSCAN::pickDevice(0x28, 0x29, false); +#elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 + secondIMUAddress = I2CSCAN::pickDevice(0x69, 0x68, false); +#else +#error Unsupported secondary IMU +#endif + + if (secondIMUAddress == firstIMUAddress) + { + m_Logger.debug("No secondary IMU connected"); + } + else if (secondIMUAddress == 0) + { + m_Sensor2 = new ErroneousSensor(1, SECOND_IMU); + } + else + { + m_Logger.trace("Secondary IMU found at address 0x%02X", secondIMUAddress); + +#if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 + m_Sensor2 = new BNO080Sensor(1, IMU, secondIMUAddress, SECOND_IMU_ROTATION, PIN_IMU_INT_2); +#elif SECOND_IMU == IMU_BNO055 + m_Sensor2 = new BNO055Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); +#elif SECOND_IMU == IMU_MPU9250 + m_Sensor2 = new MPU9250Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); +#elif SECOND_IMU == IMU_BMI160 + m_Sensor2 = new BMI160Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); +#elif SECOND_IMU == IMU_MPU6500 || IMU == IMU_MPU6050 + m_Sensor2 = new MPU6050Sensor(1, IMU, secondIMUAddress, SECOND_IMU_ROTATION); +#elif SECOND_IMU == IMU_ICM20948 + m_Sensor2 = new ICM20948Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); +#endif + } + + m_Sensor2->motionSetup(); + } + } + + void SensorManager::update() + { + // Gather IMU data + m_Sensor1->motionLoop(); + m_Sensor2->motionLoop(); + + if (!ServerConnection::isConnected()) + { + return; + } + + // Send updates + m_Sensor1->sendData(); + m_Sensor2->sendData(); + } + } +} diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h new file mode 100644 index 0000000..74dd601 --- /dev/null +++ b/src/sensors/SensorManager.h @@ -0,0 +1,62 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SLIMEVR_SENSORMANAGER +#define SLIMEVR_SENSORMANAGER + +#include "globals.h" +#include "sensor.h" +#include "EmptySensor.h" +#include "logging/Logger.h" + +namespace SlimeVR +{ + namespace Sensors + { + class SensorManager + { + public: + SensorManager() + : m_Logger(SlimeVR::Logging::Logger("SensorManager")), m_Sensor1(new EmptySensor(0)), m_Sensor2(new EmptySensor(0)) {} + ~SensorManager() + { + delete m_Sensor1; + delete m_Sensor2; + } + + void setup(); + void update(); + + Sensor *getFirst() { return m_Sensor1; }; + Sensor *getSecond() { return m_Sensor2; }; + + private: + SlimeVR::Logging::Logger m_Logger; + + Sensor *m_Sensor1; + Sensor *m_Sensor2; + }; + } +} + +#endif // SLIMEVR_SENSORFACTORY_H_ diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index d96d7b0..c0c597c 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -29,7 +29,7 @@ class BMI160Sensor : public Sensor { public: - BMI160Sensor() : Sensor("BMI160Sensor"){}; + BMI160Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("BMI160Sensor", IMU_BMI160, id, address, rotation){}; ~BMI160Sensor(){}; void motionSetup() override final; void motionLoop() override final; diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 3037615..b6eca65 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -27,7 +27,7 @@ class BNO055Sensor : public Sensor { public: - BNO055Sensor() : Sensor("BNO055Sensor"){}; + BNO055Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("BNO055Sensor", IMU_BNO055, id, address, rotation){}; ~BNO055Sensor(){}; void motionSetup() override final; void motionLoop() override final; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 695d13e..af7a422 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.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 @@ -31,7 +31,7 @@ void BNO080Sensor::motionSetup() #ifdef FULL_DEBUG imu.enableDebugging(Serial); #endif - if(!imu.begin(addr, Wire, intPin)) { + if(!imu.begin(addr, Wire, m_IntPin)) { m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr); LEDManager::signalAssert(); return; @@ -175,7 +175,7 @@ void BNO080Sensor::motionLoop() imu.getAccel(v[0], v[1], v[2], acc); Network::sendAccel(v, PACKET_ACCEL); } - if (intPin == 255 || imu.I2CTimedOut()) + if (m_IntPin == 255 || imu.I2CTimedOut()) break; } if (lastData + 1000 < millis() && configured) diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 417f359..44a4179 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -26,7 +26,8 @@ class BNO080Sensor : public Sensor { public: - BNO080Sensor() : Sensor("BNO080Sensor"){}; + BNO080Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t intPin) + : Sensor("BNO080Sensor", type, id, address, rotation), m_IntPin(intPin) {}; ~BNO080Sensor(){}; void motionSetup() override final; void motionLoop() override final; @@ -37,6 +38,8 @@ public: private: BNO080 imu{}; + uint8_t m_IntPin; + uint8_t tap; unsigned long lastData = 0; uint8_t lastReset = 0; diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index c50ccef..0c88abf 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain, SlimeVR contributors + 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 @@ -27,12 +27,13 @@ #include "sensor.h" #include // Used for periodically saving bias #ifdef ESP32 - #include // ICM bias saving. ESP8266 use eprom +#include // ICM bias saving. ESP8266 use eprom #endif -class ICM20948Sensor : public Sensor { +class ICM20948Sensor : public Sensor +{ public: - ICM20948Sensor() : Sensor("ICM20948Sensor"){} + ICM20948Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM20948Sensor", IMU_ICM20948, id, address, rotation) {} ~ICM20948Sensor() override = default; void motionSetup() override final; void motionLoop() override final; @@ -45,12 +46,12 @@ private: unsigned long lastData = 0; int bias_save_counter = 0; bool newTap; - + ICM_20948_I2C imu; ICM_20948_Device_t pdev; - icm_20948_DMP_data_t dmpData {}; + icm_20948_DMP_data_t dmpData{}; - #define OVERRIDEDMPSETUP true +#define OVERRIDEDMPSETUP true #ifdef ESP32 Preferences prefs; Timer<> timer = timer_create_default(); @@ -58,7 +59,7 @@ private: #ifdef ESP8266 Timer<> timer = timer_create_default(); #endif - //TapDetector tapDetector; + // TapDetector tapDetector; }; -#endif // SLIMEVR_ICM20948SENSOR_H_ \ No newline at end of file +#endif // SLIMEVR_ICM20948SENSOR_H_ diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index fc31aca..acf579a 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -26,7 +26,7 @@ class MPU6050Sensor : public Sensor { public: - MPU6050Sensor() : Sensor("MPU6050Sensor"){}; + MPU6050Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation) : Sensor("MPU6050Sensor", type, id, address, rotation){}; ~MPU6050Sensor(){}; void motionSetup() override final; void motionLoop() override final; @@ -42,4 +42,4 @@ 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 -}; \ No newline at end of file +}; diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index 57065eb..59ba1a7 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -28,7 +28,7 @@ class MPU9250Sensor : public Sensor { public: - MPU9250Sensor() : Sensor("MPU9250Sensor"){}; + MPU9250Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("MPU9250Sensor", IMU_MPU9250, id, address, rotation){}; ~MPU9250Sensor(){}; void motionSetup() override final; void motionLoop() override final; @@ -44,15 +44,15 @@ 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 - //raw data and scaled as vector + // raw data and scaled as vector int skipCalcMag = 0; float q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter float Axyz[3]{}; float Gxyz[3]{}; float Mxyz[3]{}; float rawMag[3]{}; - Quat correction{0,0,0,0}; + Quat correction{0, 0, 0, 0}; // Loop timing globals - unsigned long now = 0, last = 0; //micros() timers - float deltat = 0; //loop time in seconds -}; \ No newline at end of file + unsigned long now = 0, last = 0; // micros() timers + float deltat = 0; // loop time in seconds +}; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index a3175b4..730f5e6 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -25,18 +25,6 @@ #include #include "calibration.h" -void Sensor::setupSensor(uint8_t expectedSensorType, uint8_t sensorId, uint8_t addr, uint8_t intPin) { - this->sensorType = expectedSensorType; - this->addr = addr; - this->intPin = intPin; - this->sensorId = sensorId; - this->sensorOffset = {Quat(Vector3(0, 0, 1), sensorId == 0 ? IMU_ROTATION : SECOND_IMU_ROTATION)}; - - char buf[4]; - sprintf(buf, "%u", sensorId); - m_Logger.setTag(buf); -} - uint8_t Sensor::getSensorState() { return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 1b96bba..d26fb7b 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.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 @@ -38,10 +38,15 @@ class Sensor { public: - Sensor(const char *sensorName) : m_Logger(SlimeVR::Logging::Logger(sensorName)){} + Sensor(const char *sensorName, uint8_t type, uint8_t id, uint8_t address, float rotation) + : addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)) + { + char buf[4]; + sprintf(buf, "%u", id); + m_Logger.setTag(buf); + } virtual ~Sensor(){}; - void setupSensor(uint8_t expectedSensorType, uint8_t sensorId, uint8_t addr, uint8_t intPin); virtual void motionSetup(){}; virtual void motionLoop(){}; virtual void sendData(); @@ -60,14 +65,13 @@ public: protected: uint8_t addr = 0; - uint8_t intPin = 255; uint8_t sensorId = 0; uint8_t sensorType = 0; bool configured = false; bool newData = false; bool working = false; uint8_t calibrationAccuracy = 0; - Quat sensorOffset = Quat(Vector3(0, 0, 1), IMU_ROTATION); + Quat sensorOffset; Quat quaternion{}; Quat lastQuatSent{}; @@ -75,17 +79,6 @@ protected: SlimeVR::Logging::Logger m_Logger; }; -class EmptySensor : public Sensor -{ -public: - EmptySensor() : Sensor("EmptySensor"){}; - ~EmptySensor(){}; - void motionSetup() override final{}; - void motionLoop() override final{}; - void sendData() override final{}; - void startCalibration(int calibrationType) override final{}; -}; - const char * getIMUNameByType(int imuType); enum SensorStatus { diff --git a/src/sensors/sensorfactory.cpp b/src/sensors/sensorfactory.cpp deleted file mode 100644 index 786bb31..0000000 --- a/src/sensors/sensorfactory.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ -#include -#include "sensorfactory.h" - -#include "bno055sensor.h" -#include "bno080sensor.h" -#include "mpu9250sensor.h" -#include "mpu6050sensor.h" -#include "bmi160sensor.h" -#include "icm20948sensor.h" - -SensorFactory::SensorFactory() -{ -} - -SensorFactory::~SensorFactory() -{ - delete sensor1; - delete sensor2; -} - -void SensorFactory::create() -{ - uint8_t first_addr = 0; - #if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 - this->sensor1 = new BNO080Sensor(); - first_addr = I2CSCAN::pickDevice(0x4A, 0x4B, true); - #elif IMU == IMU_BNO055 - this->sensor1 = new BNO055Sensor(); - first_addr = I2CSCAN::pickDevice(0x29, 0x28, true); - #elif IMU == IMU_MPU9250 - this->sensor1 = new MPU9250Sensor(); - first_addr = I2CSCAN::pickDevice(0x68, 0x69, true); - #elif IMU == IMU_BMI160 - this->sensor1 = new BMI160Sensor(); - first_addr = I2CSCAN::pickDevice(0x68, 0x69, true); - #elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050 - this->sensor1 = new MPU6050Sensor(); - first_addr = I2CSCAN::pickDevice(0x68, 0x69, true); - #elif IMU == IMU_ICM20948 - this->sensor1 = new ICM20948Sensor(); - first_addr = I2CSCAN::pickDevice(0x68, 0x69, true); - #else - #error Unsupported IMU - #endif - - if(first_addr == 0) - this->sensor1 = new EmptySensor(); - this->sensor1->setupSensor(IMU, 0, first_addr, PIN_IMU_INT); - - uint8_t second_addr = 0; - #ifndef SECOND_IMU - this->sensor2 = new EmptySensor(); - #elif SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 - this->sensor2 = new BNO080Sensor(); - second_addr = I2CSCAN::pickDevice(0x4B, 0x4A, false); - #elif SECOND_IMU == IMU_BNO055 - this->sensor2 = new BNO055Sensor(); - second_addr = I2CSCAN::pickDevice(0x28, 0x29, false); - #elif SECOND_IMU == IMU_MPU9250 - this->sensor2 = new MPU9250Sensor(); - second_addr = I2CSCAN::pickDevice(0x69, 0x68, false); - #elif SECOND_IMU == IMU_BMI160 - this->sensor2 = new BMI160Sensor(); - second_addr = I2CSCAN::pickDevice(0x69, 0x68, false); - #elif SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 - this->sensor2 = new MPU6050Sensor(); - second_addr = I2CSCAN::pickDevice(0x69, 0x68, false); - #elif SECOND_IMU == IMU_ICM20948 - this->sensor2 = new ICM20948Sensor(); - second_addr = I2CSCAN::pickDevice(0x69, 0x68, false); - #else - #error Unsupported secondary IMU - #endif - - if(first_addr == second_addr) - this->sensor2 = new EmptySensor(); - this->sensor2->setupSensor(SECOND_IMU, 1, second_addr, PIN_IMU_INT_2); -} - -void SensorFactory::motionSetup() -{ - sensor1->motionSetup(); - #ifdef SECOND_IMU - sensor2->motionSetup(); - #endif -} - -void SensorFactory::motionLoop() -{ - sensor1->motionLoop(); - #ifdef SECOND_IMU - sensor2->motionLoop(); - #endif -} - -void SensorFactory::sendData() -{ - sensor1->sendData(); - #ifdef SECOND_IMU - sensor2->sendData(); - #endif -} - -void SensorFactory::startCalibration(int sensorId, int calibrationType) -{ - switch(sensorId) { - case 0: - sensor1->startCalibration(calibrationType); - break; - case 1: - sensor2->startCalibration(calibrationType); - break; - } -} \ No newline at end of file