Skip to content

Commit

Permalink
🪮 Moves imu definition to own service
Browse files Browse the repository at this point in the history
  • Loading branch information
@runeharlyk
runeharlyk committed Nov 14, 2024
1 parent 09f5460 commit e919b2a
Show file tree
Hide file tree
Showing 2 changed files with 90 additions and 83 deletions.
107 changes: 24 additions & 83 deletions esp32/lib/ESP32-sveltekit/Peripherals.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,19 +15,19 @@
#include <SPI.h>
#include <Wire.h>

#include <MPU6050_6Axis_MotionApps612.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADS1X15.h>
#include <NewPing.h>
#include <peripherals/imu.h>

#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"
Expand Down Expand Up @@ -100,16 +100,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
#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();
Expand Down Expand Up @@ -140,7 +131,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
void loop() {
EXECUTE_EVERY_N_MS(_updateInterval, {
beginTransaction();
updateImu();
emitIMU();
readSonar();
emitSonar();
endTransaction();
Expand Down Expand Up @@ -254,46 +245,13 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
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;
Expand Down Expand Up @@ -341,46 +299,39 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
return temperature;
}

void readSonar() {
#if FT_ENABLED(USE_USS)
_left_distance = _left_sonar->ping_cm();
delay(50);
_right_distance = _right_sonar->ping_cm();
#endif
}

float leftDistance() { return _left_distance; }
float rightDistance() { return _right_distance; }

StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;

protected:
void updateImu() {
void emitIMU() {
doc.clear();
bool newData = false;
JsonObject root = doc.to<JsonObject>();
#if FT_ENABLED(USE_IMU)
newData = imu_success && readIMU();
if (imu_success) {
doc["x"] = round2(getAngleX());
doc["y"] = round2(getAngleY());
doc["z"] = round2(getAngleZ());
}
_imu.readIMU(root);
#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();
delay(50);
_right_distance = _right_sonar->ping_cm();
#endif
serializeJson(doc, message);
socket.emit(EVENT_IMU, message);
}

void emitSonar() {
Expand All @@ -392,9 +343,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
#endif
}

float leftDistance() { return _left_distance; }
float rightDistance() { return _right_distance; }

private:
EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
FSPersistence<PeripheralsConfiguration> _persistence;
Expand All @@ -414,14 +362,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
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;
Expand Down
66 changes: 66 additions & 0 deletions esp32/lib/ESP32-sveltekit/peripherals/imu.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
#ifndef IMU_h
#define IMU_h

#include <list>
#include <SPI.h>
#include <Wire.h>
#include <ArduinoJson.h>
#include <MathUtils.h>

#include <MPU6050_6Axis_MotionApps612.h>

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

0 comments on commit e919b2a

Please sign in to comment.