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/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;
};