diff --git a/lib/bmi160/BMI160.cpp b/lib/bmi160/BMI160.cpp index 218a05f1a..1f1e6cea0 100644 --- a/lib/bmi160/BMI160.cpp +++ b/lib/bmi160/BMI160.cpp @@ -109,6 +109,12 @@ void BMI160::initialize(uint8_t addr, // I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_2, 0x00); } +void BMI160::deinitialize() { + /* Issue a soft-reset to bring the device into a clean state */ + setRegister(BMI160_RA_CMD, BMI160_CMD_SOFT_RESET); + delay(12); +} + bool BMI160::getErrReg(uint8_t* out) { bool ok = I2CdevMod::readByte(devAddr, BMI160_RA_ERR, buffer) >= 0; if (!ok) return false; diff --git a/lib/bmi160/BMI160.h b/lib/bmi160/BMI160.h index 228f9256e..9f5e0709c 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 deinitialize(); bool testConnection(); uint8_t getGyroRate(); diff --git a/src/main.cpp b/src/main.cpp index 9273451eb..71ef066dc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -30,6 +30,7 @@ #include "serial/serialcommands.h" #include "batterymonitor.h" #include "logging/Logger.h" +#include "onOffButton.h" Timer<> globalTimer; SlimeVR::Logging::Logger logger("SlimeVR"); @@ -40,6 +41,10 @@ SlimeVR::Configuration::Configuration configuration; SlimeVR::Network::Manager networkManager; SlimeVR::Network::Connection networkConnection; +#ifdef ON_OFF_BUTTON +SlimeVR::OnOffButton onOffButton; +#endif + int sensorToCalibrate = -1; bool blinking = false; unsigned long blinkStart = 0; @@ -93,6 +98,10 @@ void setup() #endif Wire.setClock(I2C_SPEED); + #ifdef ON_OFF_BUTTON + onOffButton.setup(); + #endif + // Wait for IMU to boot delay(500); @@ -118,6 +127,10 @@ void loop() sensorManager.update(); battery.Loop(); ledManager.update(); + + #ifdef ON_OFF_BUTTON + onOffButton.update(); + #endif #ifdef TARGET_LOOPTIME_MICROS long elapsed = (micros() - loopTime); if (elapsed < TARGET_LOOPTIME_MICROS) diff --git a/src/onOffButton.cpp b/src/onOffButton.cpp new file mode 100644 index 000000000..5d24ec5b4 --- /dev/null +++ b/src/onOffButton.cpp @@ -0,0 +1,99 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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. +*/ +#include "onOffButton.h" + +#include "GlobalVars.h" + +void SlimeVR::OnOffButton::setup() { +#ifdef ON_OFF_BUTTON + +#ifdef ESP8266 + digitalWrite(D0, LOW); + pinMode(D0, OUTPUT); + pinMode(ON_OFF_BUTTON, INPUT); +#endif + +#ifdef ESP32 + pinMode( + ON_OFF_BUTTON, + ON_OFF_BUTTON_ACTIVE_LEVEL == LOW ? INPUT_PULLUP : INPUT_PULLDOWN + ); + esp_deep_sleep_enable_gpio_wakeup( + 1 << ON_OFF_BUTTON, + ON_OFF_BUTTON_ACTIVE_LEVEL == LOW ? ESP_GPIO_WAKEUP_GPIO_LOW + : ESP_GPIO_WAKEUP_GPIO_HIGH + ); +#endif + +#endif +} + +void SlimeVR::OnOffButton::update() { +#ifdef ON_OFF_BUTTON + + if (digitalRead(ON_OFF_BUTTON) != ON_OFF_BUTTON_ACTIVE_LEVEL) { + return; + } + + uint32_t ringBuffer = 0; + long startTime = millis(); + while (millis() - startTime < ON_OFF_BUTTON_HOLD_TIME_MS) { + ringBuffer <<= 1; + ringBuffer |= digitalRead(ON_OFF_BUTTON) != ON_OFF_BUTTON_ACTIVE_LEVEL; + + int popCount = __builtin_popcount(ringBuffer); + if (popCount > 16) { + return; + } + delay(1); + } + + ledManager.off(); + for (int i = 0; i < 3; i++) { + ledManager.on(); + delay(100); + ledManager.off(); + delay(100); + } + + ringBuffer = 0; + while (__builtin_popcount(ringBuffer) <= 16) { + ringBuffer <<= 1; + ringBuffer |= digitalRead(ON_OFF_BUTTON) != ON_OFF_BUTTON_ACTIVE_LEVEL; + delay(1); + } + + const auto& sensors = sensorManager.getSensors(); + for (auto& sensor : sensors) { + // sensor->deinitialize(); + } + +#ifdef ESP8266 + ESP.deepSleep(0); +#endif + +#ifdef ESP32 + esp_deep_sleep_start(); +#endif +#endif +} diff --git a/src/onOffButton.h b/src/onOffButton.h new file mode 100644 index 000000000..23a5e0a0f --- /dev/null +++ b/src/onOffButton.h @@ -0,0 +1,49 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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. +*/ +#ifndef SLIMEVR_ONOFFBUTTON_H_ +#define SLIMEVR_ONOFFBUTTON_H_ + +#include "logging/Logger.h" +#include "globals.h" + +#ifndef ON_OFF_BUTTON_ACTIVE_LEVEL +#define ON_OFF_BUTTON_ACTIVE_LEVEL LOW +#endif + +#ifndef ON_OFF_BUTTON_HOLD_TIME_MS +#define ON_OFF_BUTTON_HOLD_TIME_MS 1000 +#endif + +namespace SlimeVR { + +class OnOffButton { +public: + void setup(); + void update(); +private: + SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("OnOffButton"); +}; + +} + +#endif \ No newline at end of file diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index c682c6557..f6fcbaf49 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -1006,3 +1006,7 @@ void BMI160Sensor::getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16 *z = ((int16_t)data[5] << 8) | data[4]; #endif } + +void BMI160Sensor::deinitialize() { + imu.deinitialize(); +} diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 6d842e9a8..0af989450 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -97,7 +97,7 @@ constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min( // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 16.4f // 2000 deg 0 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 32.8f // 1000 deg 1 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 65.6f // 500 deg 2 -// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg 3 +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg 3 // #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 262.4f // 125 deg 4 constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB = (16.4f * (1 << BMI160_GYRO_RANGE)); @@ -146,7 +146,7 @@ class BMI160Sensor : public Sensor { void maybeCalibrateGyro(); void maybeCalibrateAccel(); void maybeCalibrateMag(); - + void printTemperatureCalibrationState() override final; void printDebugTemperatureCalibrationState() override final; void resetTemperatureCalibrationState() override final { @@ -176,6 +176,8 @@ class BMI160Sensor : public Sensor { bool getTemperature(float* out); + void deinitialize() override final; + private: BMI160 imu {}; int axisRemap; diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index def918c48..4c5f3c3b4 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -70,3 +70,7 @@ void BNO055Sensor::motionLoop() { void BNO055Sensor::startCalibration(int calibrationType) { } + +void BNO055Sensor::deinitialize() { + imu.enterSuspendMode(); +} \ No newline at end of file diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 0035cc85e..a6ff9c389 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -40,6 +40,7 @@ class BNO055Sensor : public Sensor void motionSetup() override final; void motionLoop() override final; void startCalibration(int calibrationType) override final; + void deinitialize() override final; private: Adafruit_BNO055 imu; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index bc7594ea0..c6f10ce69 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -243,3 +243,8 @@ void BNO080Sensor::startCalibration(int calibrationType) // it's always enabled except accelerometer // that is disabled 30 seconds after startup } + +void BNO080Sensor::deinitialize() +{ + imu.softReset(); +} diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 6056121aa..d75ff0b5f 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -45,6 +45,7 @@ class BNO080Sensor : public Sensor void sendData() override final; void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; + void deinitialize() override final; protected: // forwarding constructor diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index d43af39f1..101be338b 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -758,3 +758,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) return worstResult; } #endif // OVERRIDEDMPSETUP + +void ICM20948Sensor::deinitialize() { + imu.swReset(); +} \ No newline at end of file diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 2f213e492..64ab49e42 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -77,6 +77,7 @@ class ICM20948Sensor : public Sensor void checkSensorTimeout(); void readRotation(); void readFIFOToEnd(); + void deinitialize() override final; #define OVERRIDEDMPSETUP true // TapDetector tapDetector; diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index 44ac31591..c4f3feea6 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -200,3 +200,7 @@ void MPU6050Sensor::startCalibration(int calibrationType) { ledManager.off(); } + +void MPU6050Sensor::deinitialize() { + imu.reset(); +} \ No newline at end of file diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index c5667798f..ad0ef22ab 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -40,6 +40,7 @@ class MPU6050Sensor : public Sensor void motionSetup() override final; void motionLoop() override final; void startCalibration(int calibrationType) override final; + void deinitialize() override final; private: MPU6050 imu{}; diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index a4295975f..25f56939c 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -442,3 +442,7 @@ bool MPU9250Sensor::getNextSample(union fifo_sample_raw *buffer, uint16_t *remai swapFifoData(buffer); return true; } + +void MPU9250Sensor::deinitialize() { + imu.reset(); +} \ No newline at end of file diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index 25140cb90..50a05144e 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -58,6 +58,7 @@ class MPU9250Sensor : public Sensor void motionLoop() override final; void startCalibration(int calibrationType) override final; void getMPUScaled(); + void deinitialize() override final; private: MPU9250 imu{}; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 94299f12c..4ac222f95 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -68,6 +68,7 @@ void Sensor::printTemperatureCalibrationState() { printTemperatureCalibrationUns void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +void Sensor::deinitialize() {} const char * getIMUNameByType(ImuID imuType) { switch(imuType) { @@ -106,4 +107,4 @@ const char * getIMUNameByType(ImuID imuType) { return "UNKNOWN"; } return "Unknown"; -} +} \ No newline at end of file diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index f225aebd3..221f84c22 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -90,6 +90,7 @@ class Sensor bool hasNewDataToSend() { return newFusedRotation || newAcceleration; }; + virtual void deinitialize(); protected: uint8_t addr = 0; diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index b5f49e52e..44f9307c4 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -75,13 +75,13 @@ struct BMI270 static constexpr uint8_t reg = 0x7e; static constexpr uint8_t valueSwReset = 0xb6; static constexpr uint8_t valueFifoFlush = 0xb0; - static constexpr uint8_t valueGTrigger = 0x02; + static constexpr uint8_t valueGTrigger = 0x02; }; struct PwrConf { static constexpr uint8_t reg = 0x7c; static constexpr uint8_t valueNoPowerSaving = 0x0; - static constexpr uint8_t valueFifoSelfWakeup = 0x2; + static constexpr uint8_t valueFifoSelfWakeup = 0x2; }; struct PwrCtrl { @@ -98,7 +98,7 @@ struct BMI270 }; static constexpr uint8_t InitAddr = 0x5b; - static constexpr uint8_t InitData = 0x5e; + static constexpr uint8_t InitData = 0x5e; struct InternalStatus { static constexpr uint8_t reg = 0x21; @@ -232,7 +232,7 @@ struct BMI270 // disable power saving i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); delay(1); - + // firmware upload i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); for (uint16_t pos=0; pos; - FifoBuffer read_buffer; + FifoBuffer read_buffer; template inline T getFromFifo(uint32_t &position, FifoBuffer& fifo) { @@ -373,7 +373,7 @@ struct BMI270 template void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - + const auto bytes_to_read = std::min(static_cast(read_buffer.size()), static_cast(fifo_bytes)); i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); @@ -384,7 +384,7 @@ struct BMI270 getFromFifo(i, read_buffer); // skip 1 byte } else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { - const uint8_t required_length = + const uint8_t required_length = (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) + ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) * 6; if (i - bytes_to_read < required_length) { @@ -414,6 +414,10 @@ struct BMI270 } } + void deinitialize() { + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); + } + }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index e071db55a..1c120918c 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -76,7 +76,7 @@ struct ICM42688 }; struct FifoConfig1 { static constexpr uint8_t reg = 0x5f; - static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2); //fifo accel en=1, gyro=1, temp=0 todo: fsync, hires + static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2); //fifo accel en=1, gyro=1, temp=0 todo: fsync, hires }; struct GyroConfig { static constexpr uint8_t reg = 0x4f; @@ -143,7 +143,7 @@ struct ICM42688 template void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); - + std::array read_buffer; // max 8 readings const auto bytes_to_read = std::min(static_cast(read_buffer.size()), static_cast(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize; @@ -155,10 +155,14 @@ struct ICM42688 if (entry.part.accel[0] != -32768) { processAccelSample(entry.part.accel, AccTs); - } - } + } + } } + void deinitialize() { + i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + } + }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index 785a5e325..c98424d4e 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -121,7 +121,7 @@ struct LSM6DS3TRC const auto unread_entries = read_result & 0x7ff; constexpr auto single_measurement_words = 6; constexpr auto single_measurement_bytes = sizeof(uint16_t) * single_measurement_words; - + std::array read_buffer; // max 10 packages of 6 16bit values of data form fifo const auto bytes_to_read = std::min(static_cast(read_buffer.size()), static_cast(unread_entries)) \ * sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes; @@ -133,7 +133,10 @@ struct LSM6DS3TRC } } + void deinitialize() { + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + } }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index 0dc4870f8..a5f61d13a 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -115,6 +115,10 @@ struct LSM6DSO : LSM6DSOutputHandler LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); } + void deinitialize() { + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + } + }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index ebef63ca2..1a744aa91 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -115,6 +115,10 @@ struct LSM6DSR : LSM6DSOutputHandler LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); } + void deinitialize() { + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + } + }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index ef073ecf6..638caacd6 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -130,6 +130,10 @@ struct LSM6DSV : LSM6DSOutputHandler LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); } + void deinitialize() { + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + } + }; -} // namespace \ No newline at end of file +} // namespace diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 6546784c8..3a7292d88 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -117,7 +117,7 @@ struct MPU6050 delay(100); i2c.writeReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07); // full SIGNAL_PATH_RESET: with another 100ms delay delay(100); - + // Configure i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x01); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro i2c.writeReg(MPU6050_RA_USER_CTRL, 0x00); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP @@ -164,7 +164,7 @@ struct MPU6050 i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { const FifoSample *sample = reinterpret_cast(&readBuffer[i]); - + int16_t xyz[3]; xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x); @@ -179,6 +179,10 @@ struct MPU6050 } } + void deinitialize() { + i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0xc0); // Reset the device and put it into low power mode + } + }; diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index ec076a757..c7808f878 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -229,7 +229,7 @@ class SoftFusionSensor : public Sensor } bool initResult = false; - + if constexpr(HasMotionlessCalib) { typename imu::MotionlessCalibrationData calibData; std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData)); @@ -526,6 +526,10 @@ class SoftFusionSensor : public Sensor uint32_t m_lastPollTime = micros(); uint32_t m_lastRotationPacketSent = 0; uint32_t m_lastTemperaturePacketSent = 0; + + void deinitialize() override { + m_sensor.deinitialize(); + } }; } // namespace