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

Refactor LEDManager and factor out StatusManager

Browse Source
This commit is contained in:
TheDevMinerTV
2022-03-07 21:50:07 +01:00
parent 08cc9aca76
commit e6dc2a8288
29 changed files with 526 additions and 351 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/GlobalVars.h Normal file
View File

@@ -0,0 +1,33 @@
/*
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 GLOBALVARS_H
#define GLOBALVARS_H
#include "LEDManager.h"
#include "status/StatusManager.h"
extern SlimeVR::LEDManager ledManager;
extern SlimeVR::Status::StatusManager statusManager;
#endif

215
src/LEDManager.cpp Normal file
View File

@@ -0,0 +1,215 @@
/*
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 "LEDManager.h"
#include "GlobalVars.h"
#include "status/Status.h"
namespace SlimeVR
{
void LEDManager::setup()
{
#if ENABLE_LEDS
pinMode(m_Pin, OUTPUT);
#endif
// Do the initial pull of the state
update();
}
void LEDManager::on()
{
#if ENABLE_LEDS
digitalWrite(m_Pin, LED_ON);
#endif
}
void LEDManager::off()
{
#if ENABLE_LEDS
digitalWrite(m_Pin, LED_OFF);
#endif
}
void LEDManager::blink(unsigned long time)
{
on();
delay(time);
off();
}
void LEDManager::pattern(unsigned long timeon, unsigned long timeoff, int times)
{
for (int i = 0; i < times; i++)
{
blink(timeon);
delay(timeoff);
}
}
void LEDManager::update()
{
unsigned long time = millis();
unsigned long diff = time - m_LastUpdate;
// Don't tick the LEDManager *too* often
if (diff < 10)
{
return;
}
m_LastUpdate = time;
unsigned int length = 0;
unsigned int count = 0;
if (statusManager.hasStatus(Status::LOW_BATTERY))
{
count = LOW_BATTERY_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = LOW_BATTERY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LOW_BATTERY_INTERVAL;
break;
}
}
else if (statusManager.hasStatus(Status::IMU_ERROR))
{
count = IMU_ERROR_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = IMU_ERROR_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = IMU_ERROR_INTERVAL;
break;
}
}
else if (statusManager.hasStatus(Status::WIFI_CONNECTING))
{
count = WIFI_CONNECTING_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = WIFI_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = WIFI_CONNECTING_INTERVAL;
break;
}
}
else if (statusManager.hasStatus(Status::SERVER_CONNECTING))
{
count = SERVER_CONNECTING_COUNT;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = SERVER_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = SERVER_CONNECTING_INTERVAL;
break;
}
}
else
{
#if defined(LED_INTERVAL_STANDBY) && LED_INTERVAL_STANDBY > 0
count = 1;
switch (m_CurrentStage)
{
case ON:
case OFF:
length = STANDBUY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LED_INTERVAL_STANDBY;
break;
}
#else
return;
#endif
}
if (m_CurrentStage == OFF || m_Timer + diff >= length)
{
m_Timer = 0;
// Advance stage
switch (m_CurrentStage)
{
case OFF:
on();
m_CurrentStage = ON;
m_CurrentCount = 0;
break;
case ON:
off();
m_CurrentCount++;
if (m_CurrentCount >= count)
{
m_CurrentCount = 0;
m_CurrentStage = INTERVAL;
}
else
{
m_CurrentStage = GAP;
}
break;
case GAP:
case INTERVAL:
on();
m_CurrentStage = ON;
break;
on();
m_CurrentStage = ON;
break;
}
}
else
{
m_Timer += diff;
}
}
}

77
src/ledmgr.h → src/LEDManager.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,18 +20,13 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SLIMEVR_LEDMGR_H_
#define SLIMEVR_LEDMGR_H_
#ifndef SLIMEVR_LEDMANAGER_H
#define SLIMEVR_LEDMANAGER_H
#include <Arduino.h>
#include "globals.h"
#include "logging/Logger.h"
#define LED_STATUS_SERVER_CONNECTING 2
#define LED_STATUS_WIFI_CONNECTING 4
#define LED_STATUS_LOW_BATTERY 128
#define LED_STATUS_IMU_ERROR 256
#define DEFAULT_LENGTH 300
#define DEFAULT_GAP 500
#define DEFAULT_INTERVAL 3000
@@ -50,19 +45,59 @@
#define SERVER_CONNECTING_INTERVAL 3000
#define SERVER_CONNECTING_COUNT 2
namespace LEDManager {
enum Stage {
OFF, ON, GAP, INTERVAL
namespace SlimeVR
{
enum LEDStage
{
OFF,
ON,
GAP,
INTERVAL
};
class LEDManager
{
public:
LEDManager(uint8_t pin) : m_Pin(pin) {}
void setup();
/*!
* @brief Turns the LED on
*/
void on();
/*!
* @brief Turns the LED off
*/
void off();
/*!
* @brief Blink the LED for [time]ms. *Can* cause lag
* @param time Amount of ms to turn the LED on
*/
void blink(unsigned long time);
/*!
* @brief Show a pattern on the LED. *Can* cause lag
* @param timeon Amount of ms to turn the LED on
* @param timeoff Amount of ms to turn the LED off
* @param times Amount of times to display the pattern
*/
void pattern(unsigned long timeon, unsigned long timeoff, int times);
void update();
private:
uint8_t m_CurrentCount = 0;
unsigned long m_Timer = 0;
LEDStage m_CurrentStage = OFF;
unsigned long m_LastUpdate = millis();
uint8_t m_Pin;
Logging::Logger m_Logger = Logging::Logger("LEDManager");
};
void on(uint8_t pin);
void off(uint8_t pin);
void blink(uint8_t pin, unsigned long time, uint8_t direction = LOW);
void pattern(uint8_t pin, unsigned long timeon, unsigned long timeoff, int times, uint8_t direction = LOW);
void signalAssert();
void setLedStatus(uint32_t status);
void unsetLedStatus(uint32_t status);
void ledStatusUpdate();
}
#endif // SLIMEVR_LEDMGR_H_
#endif

5
src/batterymonitor.cpp
View File

@@ -21,6 +21,7 @@
THE SOFTWARE.
*/
#include "batterymonitor.h"
#include "GlobalVars.h"
#if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021)
ADC_MODE(ADC_VCC);
@@ -119,10 +120,10 @@ void BatteryMonitor::Loop()
#if defined(BATTERY_LOW_VOLTAGE_DEEP_SLEEP) && BATTERY_LOW_VOLTAGE_DEEP_SLEEP
ESP.deepSleep(0);
#else
LEDManager::setLedStatus(LED_STATUS_LOW_BATTERY);
statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, true);
#endif
} else {
LEDManager::unsetLedStatus(LED_STATUS_LOW_BATTERY);
statusManager.setStatus(SlimeVR::Status::LOW_BATTERY, false);
}
#endif
}

1
src/batterymonitor.h
View File

@@ -28,7 +28,6 @@
#include "network/network.h"
#include <i2cscan.h>
#include <I2Cdev.h>
#include "ledmgr.h"
#include "logging/Logger.h"
#if BATTERY_MONITOR == BAT_EXTERNAL

8
src/debug.h
View File

@@ -43,13 +43,11 @@
#define serialDebug false // Set to true to get Serial output for debugging
#define serialBaudRate 115200
#define LED_INTERVAL_STANDBUY 10000
#define STATUS_PRINT_INTERVAL 15000
#define LED_INTERVAL_STANDBY 10000
#define LED_INVERTED false
#define ENABLE_LEDS true
#define LOADING_LED LED_BUILTIN
#define CALIBRATING_LED LED_BUILTIN
#define STATUS_LED LED_BUILTIN
#define LED_PIN LED_BUILTIN
// Determines how often we sample and send data
#define samplingRateInMillis 10

8
src/globals.h
View File

@@ -40,4 +40,12 @@
#define BATTERY_MONITOR BAT_INTERNAL
#endif
#if defined(LED_INVERTED) && LED_INVERTED
#define LED_ON LOW
#define LED_OFF HIGH
#else
#define LED_ON HIGH
#define LED_OFF LOW
#endif
#endif // SLIMEVR_GLOBALS_H_

250
src/ledmgr.cpp
View File

@@ -1,250 +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 "ledmgr.h"
namespace LEDManager
{
SlimeVR::Logging::Logger logger("LEDManager");
/*!
* @brief Turn a LED on
* @param pin
* LED pin
*/
void on(uint8_t pin)
{
#if ENABLE_LEDS
digitalWrite(pin, LOW);
#endif
}
/*!
* @brief Turn a LED off
* @param pin
* LED pin
*/
void off(uint8_t pin)
{
#if ENABLE_LEDS
digitalWrite(pin, HIGH);
#endif
}
/*!
* @brief Blink a LED for [time]ms
* @param pin
* LED pin
* @param time
* Amount of ms to turn the LED on
* @param direction
* Direction turn the LED on, usually LOW
*/
void blink(uint8_t pin, unsigned long time, uint8_t direction)
{
#if ENABLE_LEDS
digitalWrite(pin, direction);
delay(time);
digitalWrite(pin, direction ^ 1);
#endif
}
/*!
* @brief Show a pattern on a LED
* @param pin
* LED pin
* @param timeon
* Amount of ms to turn the LED on
* @param timeoff
* Amount of ms to turn the LED off
* @param times
* Amount of times to display the pattern
* @param direction
* Direction turn the LED on, usually LOW
*/
void pattern(uint8_t pin, unsigned long timeon, unsigned long timeoff, int times, uint8_t direction)
{
#if ENABLE_LEDS
for (int i = 0; i < times; i++)
{
digitalWrite(pin, direction);
delay(timeon);
digitalWrite(pin, direction ^ 1);
delay(timeoff);
}
#endif
}
uint32_t currentStatus = 0;
uint8_t currentCount = 0;
unsigned long timer = 0;
Stage currentStage = OFF;
unsigned long statusPrintInterval = 0;
unsigned long lastUpdate = millis();
void setLedStatus(uint32_t status) {
currentStatus |= status;
}
void unsetLedStatus(uint32_t status) {
currentStatus &= ~status;
}
void ledStatusUpdate() {
unsigned long time = millis();
unsigned long diff = time - lastUpdate;
if(diff < 10)
return;
lastUpdate = time;
unsigned int length;
unsigned int count;
bool printStatus = false;
#if defined(STATUS_PRINT_INTERVAL) && STATUS_PRINT_INTERVAL > 0
if(statusPrintInterval += diff > STATUS_PRINT_INTERVAL) {
statusPrintInterval = 0;
printStatus = true;
}
#endif
if((currentStatus & LED_STATUS_LOW_BATTERY) > 0) {
count = LOW_BATTERY_COUNT;
switch(currentStage) {
case ON:
case OFF:
length = LOW_BATTERY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LOW_BATTERY_INTERVAL;
break;
}
if(printStatus)
logger.debug("LOW BATTERY");
} else if((currentStatus & LED_STATUS_IMU_ERROR) > 0) {
count = IMU_ERROR_COUNT;
switch(currentStage) {
case ON:
case OFF:
length = IMU_ERROR_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = IMU_ERROR_INTERVAL;
break;
}
if(printStatus)
logger.debug("IMU ERROR");
} else if((currentStatus & LED_STATUS_WIFI_CONNECTING) > 0) {
count = WIFI_CONNECTING_COUNT;
switch(currentStage) {
case ON:
case OFF:
length = WIFI_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = WIFI_CONNECTING_INTERVAL;
break;
}
if(printStatus)
logger.debug("WIFI CONNECTING");
} else if((currentStatus & LED_STATUS_SERVER_CONNECTING) > 0) {
count = SERVER_CONNECTING_COUNT;
switch(currentStage) {
case ON:
case OFF:
length = SERVER_CONNECTING_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = SERVER_CONNECTING_INTERVAL;
break;
}
if(printStatus)
logger.debug("SERVER CONNECTING");
} else {
if(printStatus)
logger.debug("OK");
#if defined(LED_INTERVAL_STANDBUY) && LED_INTERVAL_STANDBUY > 0
count = 1;
switch(currentStage) {
case ON:
case OFF:
length = STANDBUY_LENGTH;
break;
case GAP:
length = DEFAULT_GAP;
break;
case INTERVAL:
length = LED_INTERVAL_STANDBUY;
break;
}
#else
return;
#endif
}
if(currentStage == OFF || timer + diff >= length) {
timer = 0;
// Advance stage
switch(currentStage) {
case OFF:
on(STATUS_LED);
currentStage = ON;
currentCount = 0;
break;
case ON:
off(STATUS_LED);
currentCount++;
if(currentCount >= count) {
currentCount = 0;
currentStage = INTERVAL;
} else {
currentStage = GAP;
}
break;
case GAP:
case INTERVAL:
on(STATUS_LED);
currentStage = ON;
break;
on(STATUS_LED);
currentStage = ON;
break;
}
} else {
timer += diff;
}
}
void signalAssert() {
pattern(LOADING_LED, 50, 50, 200);
}
}

22
src/main.cpp
View File

@@ -30,12 +30,15 @@
#include "credentials.h"
#include <i2cscan.h>
#include "serial/serialcommands.h"
#include "ledmgr.h"
#include "LEDManager.h"
#include "status/StatusManager.h"
#include "batterymonitor.h"
#include "logging/Logger.h"
SlimeVR::Logging::Logger logger("SlimeVR");
SlimeVR::Sensors::SensorManager sensorManager;
SlimeVR::LEDManager ledManager(LED_PIN);
SlimeVR::Status::StatusManager statusManager;
int sensorToCalibrate = -1;
bool blinking = false;
@@ -54,13 +57,10 @@ void setup()
logger.info("SlimeVR v" FIRMWARE_VERSION " starting up...");
//wifi_set_sleep_type(NONE_SLEEP_T);
// Glow diode while loading
#if ENABLE_LEDS
pinMode(LOADING_LED, OUTPUT);
pinMode(CALIBRATING_LED, OUTPUT);
LEDManager::off(CALIBRATING_LED);
LEDManager::on(LOADING_LED);
#endif
statusManager.setStatus(SlimeVR::Status::LOADING, true);
ledManager.setup();
SerialCommands::setUp();
@@ -84,18 +84,20 @@ void setup()
Network::setUp();
OTA::otaSetup(otaPassword);
battery.Setup();
LEDManager::off(LOADING_LED);
statusManager.setStatus(SlimeVR::Status::LOADING, false);
loopTime = micros();
}
void loop()
{
LEDManager::ledStatusUpdate();
SerialCommands::update();
OTA::otaUpdate();
Network::update(sensorManager.getFirst(), sensorManager.getSecond());
sensorManager.update();
battery.Loop();
ledManager.update();
#ifdef TARGET_LOOPTIME_MICROS
long elapsed = (micros() - loopTime);

1
src/network/network.cpp
View File

@@ -21,7 +21,6 @@
THE SOFTWARE.
*/
#include "network.h"
#include "ledmgr.h"
bool lastWifiConnected = false;

16
src/network/udpclient.cpp
View File

@@ -22,9 +22,9 @@
*/
#include "udpclient.h"
#include "ledmgr.h"
#include "packets.h"
#include "logging/Logger.h"
#include "GlobalVars.h"
#define TIMEOUT 3000UL
@@ -591,8 +591,8 @@ void ServerConnection::connect()
port = Udp.remotePort();
lastPacketMs = now;
connected = true;
LEDManager::unsetLedStatus(LED_STATUS_SERVER_CONNECTING);
LEDManager::off(LOADING_LED);
statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
ledManager.off();
udpClientLogger.debug("Handshake successful, server is %s:%d", Udp.remoteIP().toString().c_str(), + Udp.remotePort());
return;
default:
@@ -609,18 +609,19 @@ void ServerConnection::connect()
lastConnectionAttemptMs = now;
udpClientLogger.info("Looking for the server...");
Network::sendHandshake();
LEDManager::on(LOADING_LED);
ledManager.on();
}
else if(lastConnectionAttemptMs + 20 < now)
{
LEDManager::off(LOADING_LED);
ledManager.off();
}
}
void ServerConnection::resetConnection() {
Udp.begin(port);
connected = false;
LEDManager::setLedStatus(LED_STATUS_SERVER_CONNECTING);
statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
}
void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
@@ -677,7 +678,8 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
//}
if(lastPacketMs + TIMEOUT < millis())
{
LEDManager::setLedStatus(LED_STATUS_SERVER_CONNECTING);
statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
connected = false;
sensorStateNotified1 = false;
sensorStateNotified2 = false;

8
src/network/wifihandler.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
@@ -22,8 +22,8 @@
*/
#include "globals.h"
#include "network.h"
#include "ledmgr.h"
#include "logging/Logger.h"
#include "GlobalVars.h"
#if !ESP8266
#include "esp_wifi.h"
#endif
@@ -107,7 +107,7 @@ void WiFiNetwork::setUp() {
void onConnected() {
WiFiNetwork::stopProvisioning();
LEDManager::unsetLedStatus(LED_STATUS_WIFI_CONNECTING);
statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, false);
isWifiConnected = true;
hadWifi = true;
wifiHandlerLogger.info("Connected successfully to SSID '%s', ip address %s", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
@@ -120,7 +120,7 @@ void WiFiNetwork::upkeep() {
wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
isWifiConnected = false;
}
LEDManager::setLedStatus(LED_STATUS_WIFI_CONNECTING);
statusManager.setStatus(SlimeVR::Status::WIFI_CONNECTING, true);
reportWifiError();
if(wifiConnectionTimeout + 11000 < millis()) {
switch(wifiState) {

1
src/sensors/ErroneousSensor.cpp
View File

@@ -23,6 +23,7 @@
#include "ErroneousSensor.h"
#include "network/network.h"
#include "GlobalVars.h"
namespace SlimeVR
{

29
src/sensors/bmi160sensor.cpp
View File

@@ -23,6 +23,7 @@
#include "bmi160sensor.h"
#include "network/network.h"
#include "GlobalVars.h"
// Typical sensitivity at 25C
// See p. 9 of https://www.mouser.com/datasheet/2/783/BST-BMI160-DS000-1509569.pdf
@@ -59,7 +60,7 @@ void BMI160Sensor::motionSetup() {
imu.initialize(addr);
if(!imu.testConnection()) {
m_Logger.fatal("Can't connect to BMI160 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
LEDManager::signalAssert();
ledManager.pattern(50, 50, 200);
return;
}
@@ -69,7 +70,8 @@ void BMI160Sensor::motionSetup() {
imu.getAcceleration(&ax, &ay, &az);
float g_az = (float)az / 8192; // For 4G sensitivity
if(g_az < -0.75f) {
LEDManager::off(CALIBRATING_LED);
ledManager.on();
m_Logger.info("Flip front to confirm start calibration");
delay(5000);
imu.getAcceleration(&ax, &ay, &az);
@@ -80,7 +82,7 @@ void BMI160Sensor::motionSetup() {
startCalibration(0);
}
LEDManager::on(CALIBRATING_LED);
ledManager.off();
}
DeviceConfig * const config = getConfigPtr();
@@ -178,7 +180,8 @@ void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3])
}
void BMI160Sensor::startCalibration(int calibrationType) {
LEDManager::on(CALIBRATING_LED);
ledManager.on();
m_Logger.debug("Gathering raw data for device calibration...");
DeviceConfig * const config = getConfigPtr();
@@ -196,13 +199,15 @@ void BMI160Sensor::startCalibration(int calibrationType) {
for (int i = 0; i < gyroCalibrationSamples; i++)
{
LEDManager::on(CALIBRATING_LED);
ledManager.on();
int16_t gx, gy, gz;
imu.getRotation(&gx, &gy, &gz);
rawGxyz[0] += float(gx);
rawGxyz[1] += float(gy);
rawGxyz[2] += float(gz);
LEDManager::off(CALIBRATING_LED);
ledManager.off();
}
config->calibration[sensorId].G_off[0] = rawGxyz[0] / gyroCalibrationSamples;
config->calibration[sensorId].G_off[1] = rawGxyz[1] / gyroCalibrationSamples;
@@ -214,9 +219,9 @@ void BMI160Sensor::startCalibration(int calibrationType) {
// Blink calibrating led before user should rotate the sensor
m_Logger.info("After 3 seconds, Gently rotate the device while it's gathering accelerometer data");
LEDManager::on(CALIBRATING_LED);
ledManager.on();
delay(1500);
LEDManager::off(CALIBRATING_LED);
ledManager.off();
delay(1500);
m_Logger.debug("Gathering accelerometer data...");
@@ -224,16 +229,18 @@ void BMI160Sensor::startCalibration(int calibrationType) {
float *calibrationDataAcc = (float*)malloc(accelCalibrationSamples * 3 * sizeof(float));
for (int i = 0; i < accelCalibrationSamples; i++)
{
LEDManager::on(CALIBRATING_LED);
ledManager.on();
int16_t ax, ay, az;
imu.getAcceleration(&ax, &ay, &az);
calibrationDataAcc[i * 3 + 0] = ax;
calibrationDataAcc[i * 3 + 1] = ay;
calibrationDataAcc[i * 3 + 2] = az;
LEDManager::off(CALIBRATING_LED);
ledManager.off();
delay(100);
}
LEDManager::off(CALIBRATING_LED);
ledManager.off();
m_Logger.debug("Calculating calibration data...");
float A_BAinv[4][3];

9
src/sensors/bmi160sensor.h
View File

@@ -20,10 +20,13 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SENSORS_BMI160SENSOR_H
#define SENSORS_BMI160SENSOR_H
#include "sensor.h"
#include "mahony.h"
#include "magneto1.4.h"
#include "ledmgr.h"
#include <BMI160.h>
@@ -43,4 +46,6 @@ class BMI160Sensor : public Sensor {
// Loop timing globals
uint32_t now = 0, last = 0; //micros() timers
float deltat = 0; //loop time in seconds
};
};
#endif

6
src/sensors/bno055sensor.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 "bno055sensor.h"
#include "network/network.h"
#include "globals.h"
#include "ledmgr.h"
#include "GlobalVars.h"
void BNO055Sensor::motionSetup() {
imu = Adafruit_BNO055(sensorId, addr);
@@ -31,7 +31,7 @@ void BNO055Sensor::motionSetup() {
if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_IMUPLUS))
{
m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr);
LEDManager::signalAssert();
ledManager.pattern(50, 50, 200);
return;
}

8
src/sensors/bno055sensor.h
View File

@@ -20,6 +20,10 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SENSORS_BNO055SENSOR_H
#define SENSORS_BNO055SENSOR_H
#include "sensor.h"
#include <Adafruit_BNO055.h>
@@ -35,4 +39,6 @@ public:
private:
Adafruit_BNO055 imu;
};
};
#endif

14
src/sensors/bno080sensor.cpp
View File

@@ -23,8 +23,8 @@
#include "sensors/bno080sensor.h"
#include "network/network.h"
#include "ledmgr.h"
#include "utils.h"
#include "GlobalVars.h"
void BNO080Sensor::motionSetup()
{
@@ -33,7 +33,7 @@ void BNO080Sensor::motionSetup()
#endif
if(!imu.begin(addr, Wire, m_IntPin)) {
m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr);
LEDManager::signalAssert();
ledManager.pattern(50, 50, 200);
return;
}
@@ -188,7 +188,7 @@ void BNO080Sensor::motionLoop()
m_Logger.error("BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d",
error.severity, error.error_sequence_number, error.error_source, error.error, error.error_module, error.error_code);
}
LEDManager::setLedStatus(LED_STATUS_IMU_ERROR);
statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true);
working = false;
lastData = millis();
uint8_t rr = imu.resetReason();
@@ -236,16 +236,16 @@ void BNO080Sensor::sendData()
void BNO080Sensor::startCalibration(int calibrationType)
{
// TODO It only calibrates gyro, it should have multiple calibration modes, and check calibration status in motionLoop()
LEDManager::pattern(CALIBRATING_LED, 20, 20, 10);
LEDManager::blink(CALIBRATING_LED, 2000);
ledManager.pattern(20, 20, 10);
ledManager.blink(2000);
imu.calibrateGyro();
do
{
LEDManager::on(CALIBRATING_LED);
ledManager.on();
imu.requestCalibrationStatus();
delay(20);
imu.getReadings();
LEDManager::off(CALIBRATING_LED);
ledManager.off();
delay(20);
} while (!imu.calibrationComplete());
imu.saveCalibration();

8
src/sensors/bno080sensor.h
View File

@@ -20,6 +20,10 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SENSORS_BNO080SENSOR_H
#define SENSORS_BNO080SENSOR_H
#include "sensor.h"
#include <BNO080.h>
@@ -52,4 +56,6 @@ private:
bool useMagnetometerAllTheTime = false;
bool useMagnetometerCorrection = false;
bool newMagData = false;
};
};
#endif

4
src/sensors/icm20948sensor.cpp
View File

@@ -25,7 +25,7 @@
#include <i2cscan.h>
#include <EEPROM.h> // for 8266, save the current bias values to eeprom
#include "network/network.h"
#include "ledmgr.h"
#include "GlobalVars.h"
// seconds after previous save (from start) when calibration (DMP Bias) data will be saved to NVS. Increments through the list then stops; to prevent unwelcome eeprom wear.
int bias_save_periods[] = { 120, 180, 300, 600, 600 }; // 2min + 3min + 5min + 10min + 10min (no more saves after 30min)
@@ -364,7 +364,7 @@ void ICM20948Sensor::motionSetup() {
ICM_20948_Status_e imu_err = imu.begin(Wire, tracker);
if (imu_err != ICM_20948_Stat_Ok) {
m_Logger.fatal("Can't connect to ICM20948 at address 0x%02x, error code: 0x%02x", addr, imu_err);
LEDManager::signalAssert();
ledManager.pattern(50, 50, 200);
return;
}

13
src/sensors/mpu6050sensor.cpp
View File

@@ -34,7 +34,7 @@
#include <i2cscan.h>
#include "calibration.h"
#include "configuration.h"
#include "ledmgr.h"
#include "GlobalVars.h"
void MPU6050Sensor::motionSetup()
{
@@ -66,7 +66,7 @@ void MPU6050Sensor::motionSetup()
imu.PrintActiveOffsets();
#endif // IMU_MPU6050_RUNTIME_CALIBRATION
LEDManager::pattern(LOADING_LED, 50, 50, 5);
ledManager.pattern(50, 50, 5);
// turn on the DMP, now that it's ready
m_Logger.debug("Enabling DMP...");
@@ -131,7 +131,8 @@ void MPU6050Sensor::motionLoop()
}
void MPU6050Sensor::startCalibration(int calibrationType) {
LEDManager::on(CALIBRATING_LED);
ledManager.on();
#ifdef IMU_MPU6050_RUNTIME_CALIBRATION
m_Logger.info("MPU is using automatic runtime calibration. Place down the device and it should automatically calibrate after a few seconds");
@@ -145,8 +146,6 @@ void MPU6050Sensor::startCalibration(int calibrationType) {
Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_ACCEL, 0);
break;
}
LEDManager::off(CALIBRATING_LED);
#else //!IMU_MPU6050_RUNTIME_CALIBRATION
m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
delay(2000);
@@ -181,7 +180,7 @@ void MPU6050Sensor::startCalibration(int calibrationType) {
}
m_Logger.info("Calibration finished");
LEDMGR::off(CALIBRATING_LED);
#endif // !IMU_MPU6050_RUNTIME_CALIBRATION
ledManager.off();
}

6
src/sensors/mpu6050sensor.h
View File

@@ -20,6 +20,10 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SENSORS_MPU6050SENSOR_H
#define SENSORS_MPU6050SENSOR_H
#include "sensor.h"
#include <MPU6050.h>
@@ -43,3 +47,5 @@ private:
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]{}; // FIFO storage buffer
};
#endif

25
src/sensors/mpu9250sensor.cpp
View File

@@ -25,12 +25,12 @@
#include <i2cscan.h>
#include "calibration.h"
#include "magneto1.4.h"
#include "GlobalVars.h"
// #include "mahony.h"
// #include "madgwick.h"
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
#include "dmpmag.h"
#endif
#include "ledmgr.h"
constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB per d/s -> rad/s
@@ -56,10 +56,11 @@ void MPU9250Sensor::motionSetup() {
imu.getAcceleration(&ax, &ay, &az);
float g_az = (float)az / 16384; // For 2G sensitivity
if(g_az < -0.75f) {
digitalWrite(CALIBRATING_LED, HIGH);
ledManager.on();
m_Logger.info("Flip front to confirm start calibration");
delay(5000);
digitalWrite(CALIBRATING_LED, LOW);
ledManager.off();
imu.getAcceleration(&ax, &ay, &az);
g_az = (float)az / 16384;
if(g_az > 0.75f)
@@ -71,12 +72,7 @@ void MPU9250Sensor::motionSetup() {
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
devStatus = imu.dmpInitialize();
if(devStatus == 0){
for(int i = 0; i < 5; ++i) {
delay(50);
digitalWrite(LOADING_LED, LOW);
delay(50);
digitalWrite(LOADING_LED, HIGH);
}
ledManager.pattern(50, 50, 5);
// turn on the DMP, now that it's ready
m_Logger.debug("Enabling DMP...");
@@ -217,7 +213,8 @@ void MPU9250Sensor::getMPUScaled()
}
void MPU9250Sensor::startCalibration(int calibrationType) {
LEDManager::on(CALIBRATING_LED);
ledManager.on();
m_Logger.debug("Gathering raw data for device calibration...");
constexpr int calibrationSamples = 300;
DeviceConfig *config = getConfigPtr();
@@ -252,12 +249,12 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
// Blink calibrating led before user should rotate the sensor
m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data");
LEDManager::pattern(CALIBRATING_LED, 15, 300, 3000/310);
ledManager.pattern(15, 300, 3000/310);
float *calibrationDataAcc = (float*)malloc(calibrationSamples * 3 * sizeof(float));
float *calibrationDataMag = (float*)malloc(calibrationSamples * 3 * sizeof(float));
for (int i = 0; i < calibrationSamples; i++)
{
LEDManager::on(CALIBRATING_LED);
ledManager.on();
int16_t ax,ay,az,gx,gy,gz,mx,my,mz;
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
calibrationDataAcc[i * 3 + 0] = ax;
@@ -268,7 +265,7 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
calibrationDataMag[i * 3 + 2] = -mz;
Network::sendRawCalibrationData(calibrationDataAcc, CALIBRATION_TYPE_EXTERNAL_ACCEL, 0);
Network::sendRawCalibrationData(calibrationDataMag, CALIBRATION_TYPE_EXTERNAL_MAG, 0);
LEDManager::off(CALIBRATING_LED);
ledManager.off();
delay(250);
}
m_Logger.debug("Calculating calibration data...");
@@ -303,7 +300,7 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
m_Logger.debug("}");
m_Logger.debug("Now Saving EEPROM");
setConfig(*config);
LEDManager::off(CALIBRATING_LED);
ledManager.off();
Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0);
m_Logger.debug("Finished Saving EEPROM");
m_Logger.info("Calibration data gathered");

6
src/sensors/mpu9250sensor.h
View File

@@ -20,6 +20,10 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SENSORS_MPU9250SENSOR_H
#define SENSORS_MPU9250SENSOR_H
#include "sensor.h"
#include "logging/Logger.h"
@@ -56,3 +60,5 @@ private:
unsigned long now = 0, last = 0; // micros() timers
float deltat = 0; // loop time in seconds
};
#endif

26
src/status/Status.cpp Normal file
View File

@@ -0,0 +1,26 @@
#include "Status.h"
namespace SlimeVR
{
namespace Status
{
const char *statusToString(Status status)
{
switch (status)
{
case LOADING:
return "LOADING";
case LOW_BATTERY:
return "LOW_BATTERY";
case IMU_ERROR:
return "IMU_ERROR";
case WIFI_CONNECTING:
return "WIFI_CONNECTING";
case SERVER_CONNECTING:
return "SERVER_CONNECTING";
default:
return "UNKNOWN";
}
}
}
}

21
src/status/Status.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef STATUS_STATUS_H
#define STATUS_STATUS_H
namespace SlimeVR
{
namespace Status
{
enum Status
{
LOADING = 1 << 0,
LOW_BATTERY = 1 << 1,
IMU_ERROR = 1 << 2,
WIFI_CONNECTING = 1 << 3,
SERVER_CONNECTING = 1 << 4
};
const char *statusToString(Status status);
}
}
#endif

28
src/status/StatusManager.cpp Normal file
View File

@@ -0,0 +1,28 @@
#include "StatusManager.h"
namespace SlimeVR
{
namespace Status
{
void StatusManager::setStatus(Status status, bool value)
{
if (value)
{
m_Logger.trace("Removed status %s", statusToString(status));
m_Status |= status;
}
else
{
m_Logger.trace("Added status %s", statusToString(status));
m_Status &= ~status;
}
}
bool StatusManager::hasStatus(Status status)
{
return (m_Status & status) == status;
}
}
}

25
src/status/StatusManager.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef STATUS_STATUSMANAGER_H
#define STATUS_STATUSMANAGER_H
#include "Status.h"
#include "logging/Logger.h"
namespace SlimeVR
{
namespace Status
{
class StatusManager
{
public:
void setStatus(Status status, bool value);
bool hasStatus(Status status);
private:
uint32_t m_Status;
Logging::Logger m_Logger = Logging::Logger("StatusManager");
};
}
}
#endif

4
src/utils.h
View File

@@ -1,7 +1,7 @@
#ifndef UTILS_H
#define UTILS_H
#define UNPACK_VECTOR(V) V.x, V.y, V.z
#define UNPACK_QUATERNION(Q) Q.x, Q.y, Q.z, Q.w
#define UTILS_H
#endif
#endif