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

Early stages of magnetometer from ICM20948

Browse Source
This commit is contained in:
Niklas Jensen
2025-11-29 19:44:41 +01:00
parent 0d379a8013
commit 5481a598d9
4 changed files with 76 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/peripherals/imu.h
+3 -4
View File
@@ -89,10 +89,11 @@ class IMU : public SensorBase<IMUAnglesMsg> {
#if FT_ENABLED(USE_ICM20948)
#if USE_ICM20948_SPIMODE > 0
_imu = (ICM_20948_SPI*)_arg;
_imu->begin(CS_PIN, SPI_PORT);
if (true || !_imu->isConnected()) { _imu->begin(CS_PIN, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
#else
_imu = (ICM_20948_I2C*)_arg;
_imu->begin(Wire, 1, 0xFF);
if (true || !_imu->isConnected()) { _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode"); }
#endif
if (_imu->status != ICM_20948_Stat_Ok){ return false; }
@@ -105,8 +106,6 @@ class IMU : public SensorBase<IMUAnglesMsg> {
_imu->setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
if (_imu->status != ICM_20948_Stat_Ok){ return false; }
// TODO: Setup low pass filter config
_imu->startupMagnetometer();
if (_imu->status != ICM_20948_Stat_Ok){ return false; }
#endif
return true;
}
esp32/include/peripherals/magnetometer.h
+47 -10
View File
@@ -11,6 +11,7 @@
#include <peripherals/sensor.hpp>
struct MagnetometerMsg : public SensorMessageBase {
float rpy[3] {0, 0, 0};
float heading {-1};
@@ -38,20 +39,44 @@ struct MagnetometerMsg : public SensorMessageBase {
class Magnetometer : public SensorBase<MagnetometerMsg> {
public:
bool initialize(void* _) override {
_msg.success = _mag.begin();
bool initialize(void* _arg) override {
#if FT_ENABLED(USE_ICM20948)
#if USE_ICM20948_SPIMODE > 0
_mag = (ICM_20948_SPI*)_arg;
if (true || !_mag->isConnected()) { _mag->begin(CS_PIN, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode"); }
#else
_mag = (ICM_20948_I2C*)_arg;
if (true || !_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
#endif
if (_mag->status != ICM_20948_Stat_Ok){ return false; }
_mag->startupMagnetometer();
if (_mag->status != ICM_20948_Stat_Ok){ return false; }
#elif FT_ENABLED(USE_HMC5883)
_msg.success = _mag.begin();
#endif
return _msg.success;
}
bool update() override {
if (!_msg.success) return false;
sensors_event_t event;
bool updated = _mag.getEvent(&event);
if (!updated) return false;
_msg.rpy[0] = event.magnetic.x;
_msg.rpy[1] = event.magnetic.y;
_msg.rpy[2] = event.magnetic.z;
_msg.heading = atan2(event.magnetic.y, event.magnetic.x);
#if FT_ENABLED(USE_ICM20948)
_mag->getAGMT();
if (_mag->status != ICM_20948_Stat_Ok){ return false; }
_msg.rpy[0] = _mag->magX();
_msg.rpy[1] = _mag->magY();
_msg.rpy[2] = _mag->magZ();
#elif FT_ENABLED(USE_HMC5883)
sensors_event_t event;
bool updated = _mag.getEvent(&event);
if (!updated) return false;
_msg.rpy[0] = event.magnetic.x;
_msg.rpy[1] = event.magnetic.y;
_msg.rpy[2] = event.magnetic.z;
#endif
_msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
_msg.heading += declinationAngle;
if (_msg.heading < 0) _msg.heading += 2 * PI;
if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
@@ -68,6 +93,18 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
float getHeading() { return _msg.heading; }
private:
Adafruit_HMC5883_Unified _mag {12345};
#if FT_ENABLED(USE_ICM20948)
#if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
#define SPI_PORT SPI // TODO in periphearals_seetings.h
#define CS_PIN 2
ICM_20948_SPI* _mag;
#else
//#define WIRE_PORT Wire
ICM_20948_I2C* _mag;
#endif
#elif FT_ENABLED(USE_HMC5883)
Adafruit_HMC5883_Unified _mag {12345};
#endif
const float declinationAngle = 0.22;
};
esp32/include/peripherals/peripherals.h
+1 -1
View File
@@ -90,7 +90,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
IMU _imu;
#endif
#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
#if FT_ENABLED(USE_HMC5883) || FT_ENABLED(USE_ICM20948)
Magnetometer _mag;
#endif
#if FT_ENABLED(USE_BMP180)
esp32/src/peripherals/peripherals.cpp
+25 -10
View File
@@ -15,25 +15,40 @@ void Peripherals::begin() {
updatePins();
#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
#elif FT_ENABLED(USE_ICM20948)
#if FT_ENABLED(USE_ICM20948)
#if USE_ICM20948_SPIMODE > 0
ICM_20948_SPI* icm20948 = new ICM_20948_SPI;
#else
ICM_20948_I2C* icm20948 = new ICM_20948_I2C;
#endif
if (!_imu.initialize(icm20948)) ESP_LOGE("IMUService", "IMU initialize failed (ICM20948)");
#endif
#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
// --- IMU ---
#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
if (!_imu.initialize(nullptr)) ESP_LOGE("Peripherals", "IMU initialize failed");
#elif FT_ENABLED(USE_ICM20948)
if (!_imu.initialize(icm20948)) ESP_LOGE("Peripherals", "IMU initialize failed (ICM20948)");
#endif
// --- MAGNETOMETER ---
#if FT_ENABLED(USE_HMC5883)
if (!_mag.initialize(nullptr)) ESP_LOGE("Peripherals", "MAG initialize failed");
#elif FT_ENABLED(USE_ICM20948)
if (!_mag.initialize(icm20948)) ESP_LOGE("Peripherals", "MAG initialize failed (ICM20948)");
#endif
// --- BMP ---
#if FT_ENABLED(USE_BMP180)
if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
if (!_bmp.initialize(nullptr)) ESP_LOGE("Peripherals", "BMP initialize failed");
#endif
// --- GESTURE ---
#if FT_ENABLED(USE_PAJ7620U2)
if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
if (!_gesture.initialize(nullptr)) ESP_LOGE("Peripherals", "Gesture initialize failed");
#endif
// --- SONAR ---
#if FT_ENABLED(USE_USS)
_left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
_right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
@@ -96,7 +111,7 @@ bool Peripherals::readImu() {
bool Peripherals::readMag() {
bool updated = false;
#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
#if FT_ENABLED(USE_HMC5883 || USE_ICM20948)
beginTransaction();
updated = _mag.update();
endTransaction();
@@ -176,7 +191,7 @@ void Peripherals::getIMUResult(JsonVariant &root) {
JsonVariant imu = root["imu"].to<JsonVariant>();
_imu.getResults(imu);
#endif
#if FT_ENABLED(USE_HMC5883) // TODO: Add USE_ICM20948
#if FT_ENABLED(USE_HMC5883 || USE_ICM20948) // TODO:
JsonVariant mag = root["mag"].to<JsonVariant>();
_mag.getResults(mag);
#endif