Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🪮 Moves imu definition to own service

Browse Source
This commit is contained in:
Rune Harlyk
2024-11-14 10:04:49 +01:00
committed by Rune Harlyk
parent 09f5460db7
commit e919b2aa41
2 changed files with 101 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/Peripherals.h
+35 -94
View File
@@ -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"
@@ -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();
@@ -140,7 +131,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
void loop() {
EXECUTE_EVERY_N_MS(_updateInterval, {
beginTransaction();
updateImu();
emitIMU();
readSonar();
emitSonar();
endTransaction();
@@ -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;
@@ -341,40 +299,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
return temperature;
}
StatefulHttpEndpoint<PeripheralsConfiguration> 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<PeripheralsConfiguration> {
#endif
}
float leftDistance() { return _left_distance; }
float rightDistance() { return _right_distance; }
StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
void emitIMU() {
doc.clear();
JsonObject root = doc.to<JsonObject>();
#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<PeripheralsConfiguration> {
#endif
}
float leftDistance() { return _left_distance; }
float rightDistance() { return _right_distance; }
private:
EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
FSPersistence<PeripheralsConfiguration> _persistence;
@@ -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;
esp32/lib/ESP32-sveltekit/peripherals/imu.h
+66
View File
@@ -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