Rune Harlyk
91 lines
1.9 KiB
C++
91 lines
1.9 KiB
C++
#pragma once
|
|
|
|
#include <MPU6050_light.h>
|
|
#include <ArduinoJson.h>
|
|
#include <EventSocket.h>
|
|
|
|
#define IMU_INTERVAL 200
|
|
#define MAX_ESP_IMU_SIZE 250
|
|
#define EVENT_IMU "imu"
|
|
|
|
class IMUService
|
|
{
|
|
public:
|
|
IMUService(EventSocket *socket) : _socket(socket), _imu(Wire) {};
|
|
|
|
void begin()
|
|
{
|
|
byte status = _imu.begin();
|
|
imu_success = status == 0;
|
|
if(status != 0) {
|
|
ESP_LOGE("IMUService", "MPU initialize failed: %d", status);
|
|
}
|
|
_socket->registerEvent(EVENT_IMU);
|
|
calibrate();
|
|
};
|
|
|
|
bool isIMUSuccess() {
|
|
return imu_success;
|
|
}
|
|
|
|
float getTemp() {
|
|
return imu_success ? _imu.getTemp() : -1;
|
|
}
|
|
|
|
float getAngleX() {
|
|
return imu_success ? _imu.getAngleX() : -1;
|
|
}
|
|
|
|
float getAngleY() {
|
|
return imu_success ? _imu.getAngleX() : -1;
|
|
}
|
|
|
|
float getAngleZ() {
|
|
return imu_success ? _imu.getAngleZ() : -1;
|
|
}
|
|
|
|
void calibrate() {
|
|
if (imu_success) {
|
|
_imu.calcOffsets(true, true);
|
|
}
|
|
}
|
|
|
|
double round2(double value) {
|
|
return (int)(value * 100 + 0.5) / 100.0;
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
unsigned long currentMillis = millis();
|
|
|
|
if (currentMillis - _lastUpdate >= _updateInterval)
|
|
{
|
|
_lastUpdate = currentMillis;
|
|
updateImu();
|
|
}
|
|
};
|
|
|
|
protected:
|
|
JsonDocument doc;
|
|
|
|
void updateImu() {
|
|
_imu.update();
|
|
doc.clear();
|
|
doc["x"] = round2(getAngleX());
|
|
doc["y"] = round2(getAngleY());
|
|
doc["z"] = round2(getAngleZ());
|
|
doc["temp"] = round2(getTemp());
|
|
|
|
serializeJson(doc, message);
|
|
_socket->emit(EVENT_IMU, message);
|
|
}
|
|
|
|
private:
|
|
MPU6050 _imu;
|
|
EventSocket *_socket;
|
|
unsigned long _lastUpdate {0};
|
|
unsigned long _updateInterval {IMU_INTERVAL};
|
|
bool imu_success {false};
|
|
char message[MAX_ESP_IMU_SIZE];
|
|
};
|