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

Merge remote-tracking branch 'origin/master' into ICM20948_fix

Browse Source 
# Conflicts:
#	esp32/include/peripherals/imu.h
This commit is contained in:
Niklas Jensen
2025-11-27 21:14:59 +01:00
parent 52b81554a3 f78a0f50bd
commit 5b6fed69c0
3 changed files with 58 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/peripherals/imu.h
+35 -15
View File
@@ -52,22 +52,36 @@ class IMU : public SensorBase<IMUAnglesMsg> {
#if FT_ENABLED(USE_MPU6050) #if FT_ENABLED(USE_MPU6050)
_imu.initialize(); _imu.initialize();
_msg.success = _imu.testConnection(); _msg.success = _imu.testConnection();
if (!_msg.success) return false; if (!_msg.success) {
ESP_LOGE("IMU", "MPU6050 connection test failed");
return false;
}
devStatus = _imu.dmpInitialize(); devStatus = _imu.dmpInitialize();
if (devStatus == 0) { if (devStatus == 0) {
_imu.setDMPEnabled(false); _imu.setXGyroOffset(0);
_imu.setDMPConfig1(0x03); _imu.setYGyroOffset(0);
_imu.setDMPEnabled(true); _imu.setZGyroOffset(0);
_imu.setXAccelOffset(0);
_imu.setYAccelOffset(0);
_imu.setZAccelOffset(0);
_imu.setI2CMasterModeEnabled(false); _imu.setI2CMasterModeEnabled(false);
_imu.setI2CBypassEnabled(true); _imu.setI2CBypassEnabled(true);
_imu.setSleepEnabled(false); _imu.setSleepEnabled(false);
_imu.setRate(1);
_imu.resetFIFO();
_imu.setDMPEnabled(true);
ESP_LOGI("IMU", "MPU6050 DMP initialized successfully");
} else { } else {
return false; ESP_LOGE("IMU", "DMP initialization failed (code %d)", devStatus);
_msg.success = false;
} }
#endif #endif
#if FT_ENABLED(USE_BNO055) #if FT_ENABLED(USE_BNO055)
_msg.success = _imu.begin(); _msg.success = _imu.begin();
if (!_msg.success) { if (!_msg.success) {
ESP_LOGE("IMU", "BNO055 connection test failed");
return false; return false;
} }
_imu.setExtCrystalUse(true); _imu.setExtCrystalUse(true);
@@ -92,19 +106,25 @@ class IMU : public SensorBase<IMUAnglesMsg> {
_imu.startupMagnetometer(); _imu.startupMagnetometer();
if (_imu.status != ICM_20948_Stat_Ok){ return false; } if (_imu.status != ICM_20948_Stat_Ok){ return false; }
#endif #endif
return true; return _msg.success;
} }
bool update() override { bool update() override {
//if (!_msg.success) return false;
#if FT_ENABLED(USE_MPU6050) #if FT_ENABLED(USE_MPU6050)
if (_imu.dmpPacketAvailable()) { uint16_t fifoCount = _imu.getFIFOCount();
if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) { uint8_t intStatus = _imu.getIntStatus();
_imu.dmpGetQuaternion(&q, fifoBuffer);
_imu.dmpGetGravity(&gravity, &q); if (intStatus & 0x10) {
_imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity); _imu.resetFIFO();
return true; ESP_LOGW("IMU", "FIFO overflow, resetting");
} return false;
}
if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
_imu.dmpGetQuaternion(&q, fifoBuffer);
_imu.dmpGetGravity(&gravity, &q);
_imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
return true;
} }
return false; return false;
#endif #endif
@@ -148,7 +168,7 @@ class IMU : public SensorBase<IMUAnglesMsg> {
private: private:
#if FT_ENABLED(USE_MPU6050) #if FT_ENABLED(USE_MPU6050)
MPU6050 _imu; MPU6050 _imu;
uint8_t devStatus {false}; uint8_t devStatus {0};
Quaternion q; Quaternion q;
uint8_t fifoBuffer[64]; uint8_t fifoBuffer[64];
VectorFloat gravity; VectorFloat gravity;
esp32/include/peripherals/magnetometer.h
+17 -18
View File
@@ -38,37 +38,36 @@ struct MagnetometerMsg : public SensorMessageBase {
class Magnetometer : public SensorBase<MagnetometerMsg> { class Magnetometer : public SensorBase<MagnetometerMsg> {
public: public:
bool initialize() { bool initialize() override {
msg.success = _mag.begin(); _msg.success = _mag.begin();
return msg.success; return _msg.success;
} }
bool update() { bool update() override {
if (!msg.success) return false; if (!_msg.success) return false;
sensors_event_t event; sensors_event_t event;
bool updated = _mag.getEvent(&event); bool updated = _mag.getEvent(&event);
if (!updated) return false; if (!updated) return false;
msg.rpy[0] = event.magnetic.x; _msg.rpy[0] = event.magnetic.x;
msg.rpy[1] = event.magnetic.y; _msg.rpy[1] = event.magnetic.y;
msg.rpy[2] = event.magnetic.z; _msg.rpy[2] = event.magnetic.z;
msg.heading = atan2(event.magnetic.y, event.magnetic.x); _msg.heading = atan2(event.magnetic.y, event.magnetic.x);
msg.heading += declinationAngle; _msg.heading += declinationAngle;
if (msg.heading < 0) msg.heading += 2 * PI; if (_msg.heading < 0) _msg.heading += 2 * PI;
if (msg.heading > 2 * PI) msg.heading -= 2 * PI; if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
msg.heading *= 180 / M_PI; _msg.heading *= 180 / M_PI;
return true; return true;
} }
float getMagX() { return msg.rpy[0]; } float getMagX() { return _msg.rpy[0]; }
float getMagY() { return msg.rpy[1]; } float getMagY() { return _msg.rpy[1]; }
float getMagZ() { return msg.rpy[2]; } float getMagZ() { return _msg.rpy[2]; }
float getHeading() { return msg.heading; } float getHeading() { return _msg.heading; }
private: private:
Adafruit_HMC5883_Unified _mag {12345}; Adafruit_HMC5883_Unified _mag {12345};
MagnetometerMsg msg;
const float declinationAngle = 0.22; const float declinationAngle = 0.22;
}; };
esp32/src/main.cpp
+6 -6
View File
@@ -132,12 +132,6 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
#if FT_ENABLED(USE_WS2812) #if FT_ENABLED(USE_WS2812)
ledService.loop(); ledService.loop();
#endif #endif
EXECUTE_EVERY_N_MS(250, {
JsonDocument doc;
JsonVariant results = doc.to<JsonVariant>();
peripherals.getIMUResult(results);
socket.emit(EVENT_IMU, results);
});
vTaskDelayUntil(&xLastWakeTime, xFrequency); vTaskDelayUntil(&xLastWakeTime, xFrequency);
} }
} }
@@ -164,6 +158,12 @@ void IRAM_ATTR serviceLoopEntry(void *) {
wifiService.loop(); wifiService.loop();
apService.loop(); apService.loop();
EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics(socket)); EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics(socket));
EXECUTE_EVERY_N_MS(500, {
JsonDocument doc;
JsonVariant results = doc.to<JsonVariant>();
peripherals.getIMUResult(results);
socket.emit(EVENT_IMU, results);
});
vTaskDelay(100 / portTICK_PERIOD_MS); vTaskDelay(100 / portTICK_PERIOD_MS);
} }