From e919b2aa41cadf44a8e10bcc320b3ff5ee2202c8 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 14 Nov 2024 10:04:49 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=AA=AE=20Moves=20imu=20definition=20to=20?= =?UTF-8?q?own=20service?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/lib/ESP32-sveltekit/Peripherals.h | 129 ++++++-------------- esp32/lib/ESP32-sveltekit/peripherals/imu.h | 66 ++++++++++ 2 files changed, 101 insertions(+), 94 deletions(-) create mode 100644 esp32/lib/ESP32-sveltekit/peripherals/imu.h diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h index 31bf43c..00dae94 100644 --- a/esp32/lib/ESP32-sveltekit/Peripherals.h +++ b/esp32/lib/ESP32-sveltekit/Peripherals.h @@ -15,19 +15,19 @@ #include #include -#include #include #include #include #include #include #include +#include #define EVENT_CONFIGURATION_SETTINGS "peripheralSettings" #define EVENT_I2C_SCAN "i2cScan" -#define I2C_INTERVAL 5000 +#define I2C_INTERVAL 250 #define MAX_ESP_IMU_SIZE 500 #define EVENT_IMU "imu" #define EVENT_SERVO_STATE "servoState" @@ -100,16 +100,7 @@ class Peripherals : public StatefulService { #endif #if FT_ENABLED(USE_IMU) - _imu.initialize(); - imu_success = _imu.testConnection(); - devStatus = _imu.dmpInitialize(); - if (!imu_success) { - ESP_LOGE("IMUService", "MPU initialize failed"); - } - _imu.setDMPEnabled(true); - _imu.setI2CMasterModeEnabled(false); - _imu.setI2CBypassEnabled(true); - _imu.setSleepEnabled(false); + if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed"); #endif #if FT_ENABLED(USE_MAG) mag_success = _mag.begin(); @@ -140,7 +131,7 @@ class Peripherals : public StatefulService { void loop() { EXECUTE_EVERY_N_MS(_updateInterval, { beginTransaction(); - updateImu(); + emitIMU(); readSonar(); emitSonar(); endTransaction(); @@ -254,46 +245,13 @@ class Peripherals : public StatefulService { bool readIMU() { bool updated = false; #if FT_ENABLED(USE_IMU) - updated = imu_success && _imu.dmpGetCurrentFIFOPacket(fifoBuffer); - _imu.dmpGetQuaternion(&q, fifoBuffer); - _imu.dmpGetGravity(&gravity, &q); - _imu.dmpGetYawPitchRoll(ypr, &q, &gravity); + beginTransaction(); + updated = _imu.readIMU(); + endTransaction(); #endif return updated; } - float getTemp() { - float temp = -1; -#if FT_ENABLED(USE_IMU) - temp = imu_success ? imu_temperature : -1; -#endif - return temp; - } - - float getAngleX() { - float angle = 0; -#if FT_ENABLED(USE_IMU) - angle = imu_success ? ypr[0] * 180 / M_PI : 0; -#endif - return angle; - } - - float getAngleY() { - float angle = 0; -#if FT_ENABLED(USE_IMU) - angle = imu_success ? ypr[1] * 180 / M_PI : 0; -#endif - return angle; - } - - float getAngleZ() { - float angle = 0; -#if FT_ENABLED(USE_IMU) - angle = imu_success ? ypr[2] * 180 / M_PI : 0; -#endif - return angle; - } - /* MAG FUNCTIONS */ float getHeading() { float heading = 0; @@ -341,40 +299,6 @@ class Peripherals : public StatefulService { return temperature; } - StatefulHttpEndpoint endpoint; - - protected: - void updateImu() { - doc.clear(); - bool newData = false; -#if FT_ENABLED(USE_IMU) - newData = imu_success && readIMU(); - if (imu_success) { - doc["x"] = round2(getAngleX()); - doc["y"] = round2(getAngleY()); - doc["z"] = round2(getAngleZ()); - } -#endif -#if FT_ENABLED(USE_MAG) - newData = newData || mag_success; - if (mag_success) { - doc["heading"] = round2(getHeading()); - } -#endif -#if FT_ENABLED(USE_BMP) - newData = newData || bmp_success; - if (bmp_success) { - doc["pressure"] = round2(getPressure()); - doc["altitude"] = round2(getAltitude()); - doc["bmp_temp"] = round2(getTemperature()); - } -#endif - if (newData) { - serializeJson(doc, message); - socket.emit(EVENT_IMU, message); - } - } - void readSonar() { #if FT_ENABLED(USE_USS) _left_distance = _left_sonar->ping_cm(); @@ -383,6 +307,33 @@ class Peripherals : public StatefulService { #endif } + float leftDistance() { return _left_distance; } + float rightDistance() { return _right_distance; } + + StatefulHttpEndpoint endpoint; + + void emitIMU() { + doc.clear(); + JsonObject root = doc.to(); +#if FT_ENABLED(USE_IMU) + _imu.readIMU(root); +#endif +#if FT_ENABLED(USE_MAG) + if (mag_success) { + doc["heading"] = round2(getHeading()); + } +#endif +#if FT_ENABLED(USE_BMP) + if (bmp_success) { + doc["pressure"] = round2(getPressure()); + doc["altitude"] = round2(getAltitude()); + doc["bmp_temp"] = round2(getTemperature()); + } +#endif + serializeJson(doc, message); + socket.emit(EVENT_IMU, message); + } + void emitSonar() { #if FT_ENABLED(USE_USS) @@ -392,9 +343,6 @@ class Peripherals : public StatefulService { #endif } - float leftDistance() { return _left_distance; } - float rightDistance() { return _right_distance; } - private: EventEndpoint _eventEndpoint; FSPersistence _persistence; @@ -414,14 +362,7 @@ class Peripherals : public StatefulService { uint16_t target_pwm[16] = {0}; #endif #if FT_ENABLED(USE_IMU) - MPU6050 _imu; - bool imu_success {false}; - uint8_t devStatus {false}; - Quaternion q; - uint8_t fifoBuffer[64]; - VectorFloat gravity; - float ypr[3]; - float imu_temperature {-1}; + IMU _imu; #endif #if FT_ENABLED(USE_MAG) Adafruit_HMC5883_Unified _mag; diff --git a/esp32/lib/ESP32-sveltekit/peripherals/imu.h b/esp32/lib/ESP32-sveltekit/peripherals/imu.h new file mode 100644 index 0000000..5b00917 --- /dev/null +++ b/esp32/lib/ESP32-sveltekit/peripherals/imu.h @@ -0,0 +1,66 @@ +#ifndef IMU_h +#define IMU_h + +#include +#include +#include +#include +#include + +#include + +class IMU { + public: + IMU() {} + bool initialize() { + _imu.initialize(); + imu_success = _imu.testConnection(); + devStatus = _imu.dmpInitialize(); + if (!imu_success) return false; + _imu.setDMPEnabled(true); + _imu.setI2CMasterModeEnabled(false); + _imu.setI2CBypassEnabled(true); + _imu.setSleepEnabled(false); + return true; + } + + bool readIMU() { + if (!imu_success) return false; + bool updated = _imu.dmpGetCurrentFIFOPacket(fifoBuffer); + _imu.dmpGetQuaternion(&q, fifoBuffer); + _imu.dmpGetGravity(&gravity, &q); + _imu.dmpGetYawPitchRoll(ypr, &q, &gravity); + return updated; + } + + float getTemperature() { return imu_success ? imu_temperature : -1; } + + float getAngleX() { return imu_success ? ypr[0] * 180 / M_PI : 0; } + + float getAngleY() { return imu_success ? ypr[1] * 180 / M_PI : 0; } + + float getAngleZ() { return imu_success ? ypr[2] * 180 / M_PI : 0; } + + Quaternion* getQuaternion() { return &q; } + + void readIMU(JsonObject& root) { + if (!imu_success) return; + root["x"] = round2(getAngleX()); + root["y"] = round2(getAngleY()); + root["z"] = round2(getAngleZ()); + } + + bool active() { return imu_success; } + + private: + MPU6050 _imu; + bool imu_success {false}; + uint8_t devStatus {false}; + Quaternion q; + uint8_t fifoBuffer[64]; + VectorFloat gravity; + float ypr[3]; + float imu_temperature {-1}; +}; + +#endif \ No newline at end of file