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

♻️ Moves peripherals to source file, add sensor base

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-11 18:34:08 +02:00
committed by Rune Harlyk
parent 41b863a0eb
commit a5371c36b9
7 changed files with 331 additions and 270 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/peripherals/imu.h
+35 -49
View File
@@ -14,43 +14,41 @@
#include <Adafruit_BNO055.h>
#endif
struct IMUAnglesMsg {
#include <peripherals/sensor.hpp>
struct IMUAnglesMsg : public SensorMessageBase {
float rpy[3] {0, 0, 0};
float temperature {-1};
bool success {false};
friend void toJson(JsonVariant v, IMUAnglesMsg const& a) {
void toJson(JsonVariant v) const override {
JsonArray arr = v.to<JsonArray>();
arr.add(a.rpy[0]);
arr.add(a.rpy[1]);
arr.add(a.rpy[2]);
arr.add(a.temperature);
arr.add(a.success);
arr.add(rpy[0]);
arr.add(rpy[1]);
arr.add(rpy[2]);
arr.add(temperature);
arr.add(success);
}
void fromJson(JsonVariantConst o) {
JsonArrayConst arr = o.as<JsonArrayConst>();
rpy[0] = arr[0].as<float>();
rpy[1] = arr[1].as<float>();
rpy[2] = arr[2].as<float>();
temperature = arr[3].as<float>();
success = arr[4].as<bool>();
void fromJson(JsonVariantConst v) override {
JsonArrayConst arr = v.as<JsonArrayConst>();
rpy[0] = arr[0] | -1.0f;
rpy[1] = arr[1] | -1.0f;
rpy[2] = arr[2] | -1.0f;
temperature = arr[3] | -1.0f;
success = arr[4] | false;
}
friend void toJson(JsonVariant v, IMUAnglesMsg const& a) { a.toJson(v); }
};
class IMU {
class IMU : public SensorBase<IMUAnglesMsg> {
public:
IMU()
#if FT_ENABLED(USE_BNO055)
: _imu(55, 0x29)
#endif
{
}
bool initialize() {
bool initialize() override {
#if FT_ENABLED(USE_MPU6050)
_imu.initialize();
imuMsg.success = _imu.testConnection();
if (!imuMsg.success) return false;
_msg.success = _imu.testConnection();
if (!_msg.success) return false;
devStatus = _imu.dmpInitialize();
if (devStatus == 0) {
_imu.setDMPEnabled(false);
@@ -64,8 +62,8 @@ class IMU {
}
#endif
#if FT_ENABLED(USE_BNO055)
imuMsg.success = _imu.begin();
if (!imuMsg.success) {
_msg.success = _imu.begin();
if (!_msg.success) {
return false;
}
_imu.setExtCrystalUse(true);
@@ -73,14 +71,14 @@ class IMU {
return true;
}
bool readIMU() {
if (!imuMsg.success) return false;
bool update() override {
if (!_msg.success) return false;
#if FT_ENABLED(USE_MPU6050)
if (_imu.dmpPacketAvailable()) {
if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
_imu.dmpGetQuaternion(&q, fifoBuffer);
_imu.dmpGetGravity(&gravity, &q);
_imu.dmpGetYawPitchRoll(imuMsg.rpy, &q, &gravity);
_imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
return true;
}
}
@@ -89,31 +87,20 @@ class IMU {
#if FT_ENABLED(USE_BNO055)
sensors_event_t event;
_imu.getEvent(&event);
imuMsg.rpy[0] = event.orientation.x;
imuMsg.rpy[1] = event.orientation.y;
imuMsg.rpy[2] = event.orientation.z;
_msg.rpy[0] = event.orientation.x;
_msg.rpy[1] = event.orientation.y;
_msg.rpy[2] = event.orientation.z;
#endif
return true;
}
float getTemperature() { return imuMsg.temperature; }
float getTemperature() { return _msg.temperature; }
float getAngleX() { return imuMsg.rpy[2]; }
float getAngleX() { return _msg.rpy[2]; }
float getAngleY() { return imuMsg.rpy[1]; }
float getAngleY() { return _msg.rpy[1]; }
float getAngleZ() { return imuMsg.rpy[0]; }
bool isActive() { return imuMsg.success; }
IMUAnglesMsg getIMUAngles() { return imuMsg; }
void readIMU(JsonObject& root) {
if (!imuMsg.success) return;
root["x"] = round2(getAngleX());
root["y"] = round2(getAngleY());
root["z"] = round2(getAngleZ());
}
float getAngleZ() { return _msg.rpy[0]; }
private:
#if FT_ENABLED(USE_MPU6050)
@@ -124,7 +111,6 @@ class IMU {
VectorFloat gravity;
#endif
#if FT_ENABLED(USE_BNO055)
Adafruit_BNO055 _imu;
Adafruit_BNO055 _imu {55, 0x29};
#endif
IMUAnglesMsg imuMsg;
};