From a7507d02b6b6838b7646fadadbada59eb905c683 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Thu, 23 Jan 2025 23:39:54 +0100 Subject: [PATCH 1/5] Add nonblocking button support --- lib/bmi160/BMI160.cpp | 6 ++ lib/bmi160/BMI160.h | 5 +- src/button.cpp | 105 ++++++++++++++++++++ src/button.h | 62 ++++++++++++ src/debug.h | 8 ++ src/main.cpp | 11 ++ src/sensors/SensorManager.h | 6 ++ src/sensors/bmi160sensor.cpp | 2 + src/sensors/bmi160sensor.h | 2 + src/sensors/bno055sensor.cpp | 2 + src/sensors/bno055sensor.h | 1 + src/sensors/bno080sensor.cpp | 2 + src/sensors/bno080sensor.h | 1 + src/sensors/icm20948sensor.cpp | 9 +- src/sensors/icm20948sensor.h | 1 + src/sensors/mpu6050sensor.cpp | 2 + src/sensors/mpu6050sensor.h | 1 + src/sensors/mpu9250sensor.cpp | 2 + src/sensors/mpu9250sensor.h | 1 + src/sensors/sensor.h | 1 + src/sensors/softfusion/drivers/bmi270.h | 2 + src/sensors/softfusion/drivers/icm42688.h | 4 + src/sensors/softfusion/drivers/icm45base.h | 2 + src/sensors/softfusion/drivers/lsm6ds3trc.h | 2 + src/sensors/softfusion/drivers/lsm6dso.h | 2 + src/sensors/softfusion/drivers/lsm6dsr.h | 2 + src/sensors/softfusion/drivers/lsm6dsv.h | 2 + src/sensors/softfusion/drivers/mpu6050.h | 7 ++ src/sensors/softfusion/softfusionsensor.h | 7 +- 29 files changed, 252 insertions(+), 8 deletions(-) create mode 100644 src/button.cpp create mode 100644 src/button.h diff --git a/lib/bmi160/BMI160.cpp b/lib/bmi160/BMI160.cpp index 218a05f1a..6d1b4b1e3 100644 --- a/lib/bmi160/BMI160.cpp +++ b/lib/bmi160/BMI160.cpp @@ -137,6 +137,12 @@ uint8_t BMI160::getDeviceID() { return buffer[0]; } +void BMI160::deinit() { + // Issue a soft-reset to bring the device into a clean state + setRegister(BMI160_RA_CMD, BMI160_CMD_SOFT_RESET); + delay(12); +} + /** Verify the SPI connection. * Make sure the device is connected and responds as expected. * @return True if connection is valid, false otherwise diff --git a/lib/bmi160/BMI160.h b/lib/bmi160/BMI160.h index 228f9256e..943acfb69 100644 --- a/lib/bmi160/BMI160.h +++ b/lib/bmi160/BMI160.h @@ -564,6 +564,7 @@ class BMI160 { BMI160AccelRange accelRange = BMI160_ACCEL_RANGE_4G, BMI160DLPFMode accelFilterMode = BMI160_DLPF_MODE_OSR4 ); + void deinit(); bool testConnection(); uint8_t getGyroRate(); @@ -749,7 +750,7 @@ class BMI160 { uint8_t getInterruptLatch(); void setInterruptLatch(uint8_t latch); void resetInterrupt(); - + bool getGyroDrdy(); void waitForGyroDrdy(); void waitForAccelDrdy(); @@ -764,4 +765,4 @@ class BMI160 { uint8_t devAddr; }; -#endif /* _BMI160_H_ */ \ No newline at end of file +#endif /* _BMI160_H_ */ diff --git a/src/button.cpp b/src/button.cpp new file mode 100644 index 000000000..b3aa58db2 --- /dev/null +++ b/src/button.cpp @@ -0,0 +1,105 @@ +#include "button.h" + +#include +#include + +#include + +#ifdef ON_OFF_BUTTON_PIN + +void IRAM_ATTR buttonInterruptHandler() { + if (OnOffButton::getInstance().buttonPressed) { + return; + } + + OnOffButton::getInstance().signalPressStart(); +} + +void OnOffButton::setup() { + pinMode( + ON_OFF_BUTTON_PIN, + BUTTON_ACTIVE_LEVEL == 0 ? INPUT_PULLUP : INPUT_PULLDOWN + ); + + attachInterrupt( + ON_OFF_BUTTON_PIN, + buttonInterruptHandler, + BUTTON_ACTIVE_LEVEL == 0 ? FALLING : RISING + ); + + esp_deep_sleep_enable_gpio_wakeup( + 1 << ON_OFF_BUTTON_PIN, + BUTTON_ACTIVE_LEVEL == 0 ? ESP_GPIO_WAKEUP_GPIO_LOW : ESP_GPIO_WAKEUP_GPIO_HIGH + ); + +#ifdef BUTTON_IMU_ENABLE_PIN + pinMode(BUTTON_IMU_ENABLE_PIN, OUTPUT); + gpio_hold_dis(static_cast(BUTTON_IMU_ENABLE_PIN)); + digitalWrite(BUTTON_IMU_ENABLE_PIN, BUTTON_IMU_ENABLE_ACTIVE_LEVEL); +#endif + + gpio_deep_sleep_hold_en(); +} + +void OnOffButton::tick() { + if (!buttonPressed) { + return; + } + + uint64_t timeTaken = millis() - buttonPressStartMillis; + + if (getButton() && timeTaken < longPressSeconds * 1e3) { + return; + } + + if (timeTaken >= longPressSeconds * 1e3) { + emitOnBeforeSleep(); + +#ifdef BUTTON_IMU_ENABLE_PIN + digitalWrite(BUTTON_IMU_ENABLE_PIN, LOW); + gpio_hold_en(static_cast(BUTTON_IMU_ENABLE_PIN)); +#endif + + ledManager.pattern(100, 100, 3); + + while (getButton()) + ; + + esp_deep_sleep_start(); + } + + buttonPressed = false; +} + +void OnOffButton::onBeforeSleep(std::function callback) { + callbacks.push_back(callback); +} + +OnOffButton& OnOffButton::getInstance() { return instance; } + +bool OnOffButton::getButton() { + static constexpr uint8_t circularBufferBitCount + = sizeof(buttonCircularBuffer) * CHAR_BIT; + + bool isPressed = digitalRead(ON_OFF_BUTTON_PIN) == BUTTON_ACTIVE_LEVEL; + buttonCircularBuffer = buttonCircularBuffer << 1 | isPressed; + + auto popCount = __builtin_popcount(buttonCircularBuffer); + return popCount >= circularBufferBitCount / 2; +} + +void OnOffButton::signalPressStart() { + buttonPressed = true; + buttonPressStartMillis = millis(); + buttonCircularBuffer = UINT64_MAX; +} + +void OnOffButton::emitOnBeforeSleep() { + for (auto& callback : callbacks) { + callback(); + } +} + +OnOffButton OnOffButton::instance; + +#endif diff --git a/src/button.h b/src/button.h new file mode 100644 index 000000000..39e682e31 --- /dev/null +++ b/src/button.h @@ -0,0 +1,62 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Gorbit99 & 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. +*/ + +#pragma once + +#include + +#include +#include +#include + +#include "GlobalVars.h" + +#ifdef ON_OFF_BUTTON_PIN + +class OnOffButton { +public: + void setup(); + void tick(); + void onBeforeSleep(std::function callback); + + static OnOffButton& getInstance(); + +private: + OnOffButton() = default; + static OnOffButton instance; + + static constexpr float longPressSeconds = 1.0f; + + bool getButton(); + void signalPressStart(); + void emitOnBeforeSleep(); + + bool buttonPressed = false; + uint64_t buttonPressStartMillis = 0; + uint64_t buttonCircularBuffer = 0; + std::vector> callbacks; + + friend void IRAM_ATTR buttonInterruptHandler(); +}; + +#endif diff --git a/src/debug.h b/src/debug.h index 9e0c7aa2e..f8f8db1bc 100644 --- a/src/debug.h +++ b/src/debug.h @@ -100,4 +100,12 @@ #define FIRMWARE_VERSION "UNKNOWN" #endif +#ifndef BUTTON_ACTIVE_LEVEL +#define BUTTON_ACTIVE_LEVEL 0 +#endif + +#ifndef BUTTON_IMU_ENABLE_ACTIVE_LEVEL +#define BUTTON_IMU_ENABLE_ACTIVE_LEVEL 1 +#endif + #endif // SLIMEVR_DEBUG_H_ diff --git a/src/main.cpp b/src/main.cpp index a5f4eb88c..5b8ee4d6f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -26,6 +26,7 @@ #include "GlobalVars.h" #include "Wire.h" #include "batterymonitor.h" +#include "button.h" #include "credentials.h" #include "globals.h" #include "logging/Logger.h" @@ -59,6 +60,11 @@ void setup() { Serial.println(); Serial.println(); +#ifdef ON_OFF_BUTTON_PIN + OnOffButton::getInstance().setup(); + OnOffButton::getInstance().onBeforeSleep([]() { sensorManager.deinitAll(); }); +#endif + logger.info("SlimeVR v" FIRMWARE_VERSION " starting up..."); statusManager.setStatus(SlimeVR::Status::LOADING, true); @@ -123,6 +129,11 @@ void loop() { battery.Loop(); ledManager.update(); I2CSCAN::update(); + +#ifdef ON_OFF_BUTTON_PIN + OnOffButton::getInstance().tick(); +#endif + #ifdef TARGET_LOOPTIME_MICROS long elapsed = (micros() - loopTime); if (elapsed < TARGET_LOOPTIME_MICROS) { diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index da590f1ec..99681516c 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -65,6 +65,12 @@ class SensorManager { return SensorTypeID::Unknown; } + void deinitAll() { + for (auto& sensor : m_Sensors) { + sensor->deinit(); + } + } + private: SlimeVR::Logging::Logger m_Logger; diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index ae6c0a6e1..2bd695a15 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -1188,3 +1188,5 @@ void BMI160Sensor::getMagnetometerXYZFromBuffer( *z = ((int16_t)data[5] << 8) | data[4]; #endif } + +void BMI160Sensor::deinit() { imu.deinit(); } diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index b0aa0f417..930a14fc6 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -208,6 +208,8 @@ class BMI160Sensor : public Sensor { bool getTemperature(float* out); + void deinit() final; + private: BMI160 imu{}; int axisRemap; diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 27953260f..97fd35bfa 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -89,3 +89,5 @@ void BNO055Sensor::motionLoop() { } void BNO055Sensor::startCalibration(int calibrationType) {} + +void BNO055Sensor::deinit() { imu.enterSuspendMode(); } diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 16fb6b020..d44bcfb5d 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -53,6 +53,7 @@ class BNO055Sensor : public Sensor { void motionSetup() override final; void motionLoop() override final; void startCalibration(int calibrationType) override final; + void deinit() final; private: Adafruit_BNO055 imu; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index b90830848..197032a1d 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -376,3 +376,5 @@ void BNO080Sensor::startCalibration(int calibrationType) { // it's always enabled except accelerometer // that is disabled 30 seconds after startup } + +void BNO080Sensor::deinit() { imu.softReset(); } diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 19072db8b..827375bdd 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -61,6 +61,7 @@ class BNO080Sensor : public Sensor { void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; void setFlag(uint16_t flagId, bool state) override final; + void deinit() final; protected: // forwarding constructor diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 2d66cb301..2e91558e9 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -31,8 +31,11 @@ // 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) + = {120, + 180, + 300, + 600, + 600}; // 2min + 3min + 5min + 10min + 10min (no more saves after 30min) #define ACCEL_SENSITIVITY_4G 8192.0f @@ -993,3 +996,5 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) { return worstResult; } #endif // OVERRIDEDMPSETUP + +void ICM20948Sensor::deinit() { imu.swReset(); } diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 469b67160..be4a7a798 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -89,6 +89,7 @@ class ICM20948Sensor : public Sensor { void checkSensorTimeout(); void readRotation(); void readFIFOToEnd(); + void deinit() final; #define OVERRIDEDMPSETUP true // TapDetector tapDetector; diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index e3402e0c3..9224db019 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -236,3 +236,5 @@ void MPU6050Sensor::startCalibration(int calibrationType) { ledManager.off(); } + +void MPU6050Sensor::deinit() { imu.reset(); } diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index d81f43c6c..d693089bb 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -54,6 +54,7 @@ class MPU6050Sensor : public Sensor { void motionSetup() override final; void motionLoop() override final; void startCalibration(int calibrationType) override final; + void deinit() final; private: MPU6050 imu{}; diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 6425d7b73..2ff681414 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -520,3 +520,5 @@ bool MPU9250Sensor::getNextSample( swapFifoData(buffer); return true; } + +void MPU9250Sensor::deinit() { imu.reset(); } diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index b8e9d5724..4a8fc4707 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -72,6 +72,7 @@ class MPU9250Sensor : public Sensor { void motionLoop() override final; void startCalibration(int calibrationType) override final; void getMPUScaled(); + void deinit() final; private: MPU9250 imu{}; diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 012a5d162..a040fdad4 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -114,6 +114,7 @@ class Sensor { TPSCounter m_tpsCounter; TPSCounter m_dataCounter; SlimeVR::SensorInterface* m_hwInterface = nullptr; + virtual void deinit() {} protected: uint8_t addr = 0; diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index d5d1099b0..f9b433a41 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -446,6 +446,8 @@ struct BMI270 { } } } + + void deinit() { i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index eab654eab..43711eb3a 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -168,6 +168,10 @@ struct ICM42688 { } } } + + void deinit() { + i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 47dc5b1cd..bf9e6c59b 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -186,6 +186,8 @@ struct ICM45Base { } } } + + void deinit() { softResetIMU(); } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index cf0f31975..deffaa5ac 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -147,6 +147,8 @@ struct LSM6DS3TRC { ); } } + + void deinit() { i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index e2b03d9e6..1a8015bdb 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -116,6 +116,8 @@ struct LSM6DSO : LSM6DSOutputHandler { AccTs ); } + + void deinit() { i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index a91e24262..cf12fc37d 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -116,6 +116,8 @@ struct LSM6DSR : LSM6DSOutputHandler { AccTs ); } + + void deinit() { i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 6127d62fa..59975d34c 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -131,6 +131,8 @@ struct LSM6DSV : LSM6DSOutputHandler { AccTs ); } + + void deinit() { i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 8fb2ce9b9..c5484af17 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -205,6 +205,13 @@ struct MPU6050 { processGyroSample(xyz, GyrTs); } } + + void deinit() { + i2c.writeReg( + MPU6050_RA_PWR_MGMT_1, + 0xc0 + ); // Reset the device and put it into low power mode + } }; } // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 1093ce97f..a1e5424df 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -136,8 +136,7 @@ class SoftFusionSensor : public Sensor { ), static_cast( GScale * (static_cast(xyz[2]) - m_calibration.G_off[2]) - ) - }; + )}; m_fusion.updateGyro(scaledData, m_calibration.G_Ts); } @@ -637,8 +636,7 @@ class SoftFusionSensor : public Sensor { .G_Ts = imu::GyrTs, .M_Ts = imu::MagTs, .G_Sens = {1.0, 1.0, 1.0}, - .MotionlessData = {} - }; + .MotionlessData = {}}; SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros(); @@ -647,6 +645,7 @@ class SoftFusionSensor : public Sensor { uint32_t m_lastTemperaturePacketSent = 0; RestCalibrationDetector calibrationDetector; + void deinit() final { m_sensor.deinit(); } }; } // namespace SlimeVR::Sensors From 4fe697d2907173d122db3551d89e0994430e091a Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 24 Jan 2025 00:16:45 +0100 Subject: [PATCH 2/5] Autosleep after inactivity --- src/button.cpp | 38 +++++++++++++++-------- src/button.h | 4 +++ src/main.cpp | 4 +++ src/sensors/SensorManager.h | 10 ++++++ src/sensors/bmi160sensor.cpp | 2 ++ src/sensors/bmi160sensor.h | 1 + src/sensors/bno080sensor.cpp | 6 ++++ src/sensors/bno080sensor.h | 1 + src/sensors/sensor.h | 1 + src/sensors/softfusion/softfusionsensor.h | 1 + 10 files changed, 55 insertions(+), 13 deletions(-) diff --git a/src/button.cpp b/src/button.cpp index b3aa58db2..bcbdae2de 100644 --- a/src/button.cpp +++ b/src/button.cpp @@ -42,6 +42,12 @@ void OnOffButton::setup() { } void OnOffButton::tick() { + uint64_t elapsed = millis() - lastActivityMillis; + + if (elapsed >= BUTTON_AUTO_SLEEP_TIME_SECONDS * 1e3) { + goToSleep(); + } + if (!buttonPressed) { return; } @@ -53,19 +59,7 @@ void OnOffButton::tick() { } if (timeTaken >= longPressSeconds * 1e3) { - emitOnBeforeSleep(); - -#ifdef BUTTON_IMU_ENABLE_PIN - digitalWrite(BUTTON_IMU_ENABLE_PIN, LOW); - gpio_hold_en(static_cast(BUTTON_IMU_ENABLE_PIN)); -#endif - - ledManager.pattern(100, 100, 3); - - while (getButton()) - ; - - esp_deep_sleep_start(); + goToSleep(); } buttonPressed = false; @@ -75,6 +69,8 @@ void OnOffButton::onBeforeSleep(std::function callback) { callbacks.push_back(callback); } +void OnOffButton::signalTrackerMoved() { lastActivityMillis = millis(); } + OnOffButton& OnOffButton::getInstance() { return instance; } bool OnOffButton::getButton() { @@ -100,6 +96,22 @@ void OnOffButton::emitOnBeforeSleep() { } } +void OnOffButton::goToSleep() { + emitOnBeforeSleep(); + +#ifdef BUTTON_IMU_ENABLE_PIN + digitalWrite(BUTTON_IMU_ENABLE_PIN, LOW); + gpio_hold_en(static_cast(BUTTON_IMU_ENABLE_PIN)); +#endif + + ledManager.pattern(100, 100, 3); + + while (buttonPressed && getButton()) + ; + + esp_deep_sleep_start(); +} + OnOffButton OnOffButton::instance; #endif diff --git a/src/button.h b/src/button.h index 39e682e31..57f5d93fa 100644 --- a/src/button.h +++ b/src/button.h @@ -38,6 +38,7 @@ class OnOffButton { void setup(); void tick(); void onBeforeSleep(std::function callback); + void signalTrackerMoved(); static OnOffButton& getInstance(); @@ -50,12 +51,15 @@ class OnOffButton { bool getButton(); void signalPressStart(); void emitOnBeforeSleep(); + void goToSleep(); bool buttonPressed = false; uint64_t buttonPressStartMillis = 0; uint64_t buttonCircularBuffer = 0; std::vector> callbacks; + uint64_t lastActivityMillis = 0; + friend void IRAM_ATTR buttonInterruptHandler(); }; diff --git a/src/main.cpp b/src/main.cpp index 5b8ee4d6f..712677b18 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -132,6 +132,10 @@ void loop() { #ifdef ON_OFF_BUTTON_PIN OnOffButton::getInstance().tick(); + + if (!sensorManager.allAtRest()) { + OnOffButton::getInstance().signalTrackerMoved(); + } #endif #ifdef TARGET_LOOPTIME_MICROS diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 99681516c..9e06cd2e9 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -71,6 +71,16 @@ class SensorManager { } } + bool allAtRest() { + for (auto& sensor : m_Sensors) { + if (!sensor->isAtRest()) { + return false; + } + } + + return true; + } + private: SlimeVR::Logging::Logger m_Logger; diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 2bd695a15..4d0906c0e 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -1190,3 +1190,5 @@ void BMI160Sensor::getMagnetometerXYZFromBuffer( } void BMI160Sensor::deinit() { imu.deinit(); } + +bool BMI160Sensor::isAtRest() { return sfusion.getRestDetected(); } diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 930a14fc6..ff5968191 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -209,6 +209,7 @@ class BMI160Sensor : public Sensor { bool getTemperature(float* out); void deinit() final; + bool isAtRest() final; private: BMI160 imu{}; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 197032a1d..908e0bb3a 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -139,6 +139,10 @@ void BNO080Sensor::motionSetup() { imu.enableStabilityClassifier(500); +#ifdef BUTTON_AUTO_SLEEP_TIME_SECONDS + imu.enableStabilityClassifier(500); +#endif + lastReset = 0; lastData = millis(); working = true; @@ -378,3 +382,5 @@ void BNO080Sensor::startCalibration(int calibrationType) { } void BNO080Sensor::deinit() { imu.softReset(); } + +bool BNO080Sensor::isAtRest() { return imu.getStabilityClassifier() == 1; } diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 827375bdd..624d16261 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -62,6 +62,7 @@ class BNO080Sensor : public Sensor { SensorStatus getSensorState() override final; void setFlag(uint16_t flagId, bool state) override final; void deinit() final; + bool isAtRest() final; protected: // forwarding constructor diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index a040fdad4..ade7b6ec6 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -115,6 +115,7 @@ class Sensor { TPSCounter m_dataCounter; SlimeVR::SensorInterface* m_hwInterface = nullptr; virtual void deinit() {} + virtual bool isAtRest() { return false; } protected: uint8_t addr = 0; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index a1e5424df..d397dabba 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -646,6 +646,7 @@ class SoftFusionSensor : public Sensor { RestCalibrationDetector calibrationDetector; void deinit() final { m_sensor.deinit(); } + bool isAtRest() final { return m_fusion.getRestDetected(); } }; } // namespace SlimeVR::Sensors From c5cbbcabebe1e1231b59c56ed8e9f0f8b67e7f7f Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 24 Jan 2025 00:29:24 +0100 Subject: [PATCH 3/5] Auto sleep when the battery voltage is too low --- src/button.cpp | 25 +++++++++++++++++++++++-- src/button.h | 3 +++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/button.cpp b/src/button.cpp index bcbdae2de..66f17e5cf 100644 --- a/src/button.cpp +++ b/src/button.cpp @@ -5,6 +5,8 @@ #include +#include "GlobalVars.h" + #ifdef ON_OFF_BUTTON_PIN void IRAM_ATTR buttonInterruptHandler() { @@ -42,11 +44,30 @@ void OnOffButton::setup() { } void OnOffButton::tick() { - uint64_t elapsed = millis() - lastActivityMillis; +#ifdef BUTTON_AUTO_SLEEP_TIME_SECONDS + uint64_t autoSleepElapsed = millis() - lastActivityMillis; - if (elapsed >= BUTTON_AUTO_SLEEP_TIME_SECONDS * 1e3) { + if (autoSleepElapsed >= BUTTON_AUTO_SLEEP_TIME_SECONDS * 1e3) { goToSleep(); } +#endif + +#ifdef BUTTON_BATTERY_VOLTAGE_THRESHOLD + if (battery.getVoltage() >= BUTTON_BATTERY_VOLTAGE_THRESHOLD) { + batteryBad = false; + } else if (!batteryBad) { + batteryBad = true; + batteryBadSinceMillis = millis(); + } + + if (batteryBad) { + uint64_t batteryBadElapsed = millis() - batteryBadSinceMillis; + + if (batteryBadElapsed >= batteryBadTimeoutSeconds * 1e3) { + goToSleep(); + } + } +#endif if (!buttonPressed) { return; diff --git a/src/button.h b/src/button.h index 57f5d93fa..780ee328e 100644 --- a/src/button.h +++ b/src/button.h @@ -47,6 +47,7 @@ class OnOffButton { static OnOffButton instance; static constexpr float longPressSeconds = 1.0f; + static constexpr float batteryBadTimeoutSeconds = 10.0f; bool getButton(); void signalPressStart(); @@ -57,6 +58,8 @@ class OnOffButton { uint64_t buttonPressStartMillis = 0; uint64_t buttonCircularBuffer = 0; std::vector> callbacks; + bool batteryBad = false; + uint64_t batteryBadSinceMillis = 0; uint64_t lastActivityMillis = 0; From 2df8780ffa8f816452d8bed2188f6f671576ccb5 Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 24 Jan 2025 00:53:31 +0100 Subject: [PATCH 4/5] Make code work for esp8266 --- src/button.cpp | 34 ++++++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 10 deletions(-) diff --git a/src/button.cpp b/src/button.cpp index 66f17e5cf..2519f75a2 100644 --- a/src/button.cpp +++ b/src/button.cpp @@ -1,9 +1,11 @@ #include "button.h" +#include + +#if ESP32 #include #include - -#include +#endif #include "GlobalVars.h" @@ -18,17 +20,18 @@ void IRAM_ATTR buttonInterruptHandler() { } void OnOffButton::setup() { +#if ESP8266 + digitalWrite(D0, LOW); + pinMode(D0, OUTPUT); + pinMode(ON_OFF_BUTTON_PIN, INPUT); +#endif + +#if ESP32 pinMode( ON_OFF_BUTTON_PIN, BUTTON_ACTIVE_LEVEL == 0 ? INPUT_PULLUP : INPUT_PULLDOWN ); - attachInterrupt( - ON_OFF_BUTTON_PIN, - buttonInterruptHandler, - BUTTON_ACTIVE_LEVEL == 0 ? FALLING : RISING - ); - esp_deep_sleep_enable_gpio_wakeup( 1 << ON_OFF_BUTTON_PIN, BUTTON_ACTIVE_LEVEL == 0 ? ESP_GPIO_WAKEUP_GPIO_LOW : ESP_GPIO_WAKEUP_GPIO_HIGH @@ -41,6 +44,13 @@ void OnOffButton::setup() { #endif gpio_deep_sleep_hold_en(); +#endif + + attachInterrupt( + ON_OFF_BUTTON_PIN, + buttonInterruptHandler, + BUTTON_ACTIVE_LEVEL == 0 ? FALLING : RISING + ); } void OnOffButton::tick() { @@ -52,7 +62,7 @@ void OnOffButton::tick() { } #endif -#ifdef BUTTON_BATTERY_VOLTAGE_THRESHOLD +#if defined(BUTTON_BATTERY_VOLTAGE_THRESHOLD) && BATTERY_MONITOR == BAT_EXTERNAL if (battery.getVoltage() >= BUTTON_BATTERY_VOLTAGE_THRESHOLD) { batteryBad = false; } else if (!batteryBad) { @@ -120,7 +130,7 @@ void OnOffButton::emitOnBeforeSleep() { void OnOffButton::goToSleep() { emitOnBeforeSleep(); -#ifdef BUTTON_IMU_ENABLE_PIN +#if defined(BUTTON_IMU_ENABLE_PIN) && ESP32 digitalWrite(BUTTON_IMU_ENABLE_PIN, LOW); gpio_hold_en(static_cast(BUTTON_IMU_ENABLE_PIN)); #endif @@ -130,7 +140,11 @@ void OnOffButton::goToSleep() { while (buttonPressed && getButton()) ; +#if ESP8266 + ESP.deepSleep(0); +#elif ESP32 esp_deep_sleep_start(); +#endif } OnOffButton OnOffButton::instance; From 56f3d0f7a775253862ffecc0b14f4c950f5e76bd Mon Sep 17 00:00:00 2001 From: gorbit99 Date: Fri, 24 Jan 2025 01:12:41 +0100 Subject: [PATCH 5/5] Format --- src/sensors/icm20948sensor.cpp | 7 ++----- src/sensors/softfusion/softfusionsensor.h | 6 ++++-- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 2e91558e9..f4e122908 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -31,11 +31,8 @@ // 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) + = {120, 180, 300, 600, 600 +}; // 2min + 3min + 5min + 10min + 10min (no more saves after 30min) #define ACCEL_SENSITIVITY_4G 8192.0f diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index d397dabba..5fa187949 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -136,7 +136,8 @@ class SoftFusionSensor : public Sensor { ), static_cast( GScale * (static_cast(xyz[2]) - m_calibration.G_off[2]) - )}; + ) + }; m_fusion.updateGyro(scaledData, m_calibration.G_Ts); } @@ -636,7 +637,8 @@ class SoftFusionSensor : public Sensor { .G_Ts = imu::GyrTs, .M_Ts = imu::MagTs, .G_Sens = {1.0, 1.0, 1.0}, - .MotionlessData = {}}; + .MotionlessData = {} + }; SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros();