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

Rewrite the SensorFactory

Browse Source
This commit is contained in:
TheDevMinerTV
2022-03-06 17:05:18 +01:00
parent 810a2169b5
commit 84c2b4e0dd
18 changed files with 422 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/debug.h
View File

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

29
src/main.cpp
View File

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

93
src/network/udpclient.cpp
View File

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

40
src/sensors/sensorfactory.h → src/sensors/EmptySensor.h
View File

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

41
src/sensors/ErroneousSensor.cpp Normal file
View File

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

51
src/sensors/ErroneousSensor.h Normal file
View File

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

139
src/sensors/SensorManager.cpp Normal file
View File

@@ -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 <i2cscan.h>
#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();
}
}
}

62
src/sensors/SensorManager.h Normal file
View File

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

2
src/sensors/bmi160sensor.h
View File

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

2
src/sensors/bno055sensor.h
View File

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

6
src/sensors/bno080sensor.cpp
View File

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

5
src/sensors/bno080sensor.h
View File

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

19
src/sensors/icm20948sensor.h
View File

@@ -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 <arduino-timer.h> // Used for periodically saving bias
#ifdef ESP32
#include <Preferences.h> // ICM bias saving. ESP8266 use eprom
#include <Preferences.h> // 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_
#endif // SLIMEVR_ICM20948SENSOR_H_

4
src/sensors/mpu6050sensor.h
View File

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

12
src/sensors/mpu9250sensor.h
View File

@@ -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
};
unsigned long now = 0, last = 0; // micros() timers
float deltat = 0; // loop time in seconds
};

12
src/sensors/sensor.cpp
View File

@@ -25,18 +25,6 @@
#include <i2cscan.h>
#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;
}

25
src/sensors/sensor.h
View File

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

136
src/sensors/sensorfactory.cpp
View File

@@ -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 <i2cscan.h>
#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;
}
}