🧼 Makes magnometer own class

esp32/include/spot.h

@@ -48,7 +48,10 @@ class Spot {
     void initialize();
 
     // sense
-    void readSensors() { _peripherals.readIMU(); }
+    void readSensors() {
+        _peripherals.readIMU();
+        _peripherals.readMag();
+    }
 
     // plan
     void planMotion() { updatedMotion = _motionService.updateMotion(); }
@@ -66,7 +69,6 @@ class Spot {
     // communicate
     void emitTelemetry() {
         if (updatedMotion) EXECUTE_EVERY_N_MS(100, { _motionService.emitAngles(); });
-        // _peripherals.loop();
         EXECUTE_EVERY_N_MS(1000, { _peripherals.emitIMU(); });
         // _peripherals.emitSonar();
     }

esp32/lib/ESP32-sveltekit/peripherals/imu.h

@@ -1,5 +1,4 @@
-#ifndef IMU_h
+#pragma once
-#define IMU_h
 
 #include <list>
 #include <SPI.h>
@@ -62,5 +61,3 @@ class IMU {
     float ypr[3];
     float imu_temperature {-1};
 };
-
-#endif

esp32/lib/ESP32-sveltekit/peripherals/magnetometer.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <utils/math_utils.h>
+
+#include <Adafruit_HMC5883_U.h>
+#include <Adafruit_Sensor.h>
+
+class Magnetometer {
+  public:
+    Magnetometer() : _mag(12345) {}
+    bool initialize() { return _mag.begin(); }
+
+    bool readMagnetometer() {
+        if (!mag_success) return false;
+        sensors_event_t event;
+        bool updated = _mag.getEvent(&event);
+        if (!updated) return false;
+        ypr[0] = event.magnetic.x;
+        ypr[1] = event.magnetic.y;
+        ypr[2] = event.magnetic.z;
+        heading = atan2(event.magnetic.y, event.magnetic.x);
+        heading += declinationAngle;
+        if (heading < 0) heading += 2 * PI;
+        if (heading > 2 * PI) heading -= 2 * PI;
+        heading *= 180 / M_PI;
+        return true;
+    }
+
+    float getMagX() { return mag_success ? ypr[0] : 0; }
+
+    float getMagY() { return mag_success ? ypr[1] : 0; }
+
+    float getMagZ() { return mag_success ? ypr[2] : 0; }
+
+    float getHeading() { return heading; }
+
+    void readMagnetometer(JsonObject& root) {
+        if (!mag_success) return;
+        root["heading"] = round2(getHeading());
+    }
+
+    bool active() { return mag_success; }
+
+  private:
+    Adafruit_HMC5883_Unified _mag;
+    bool mag_success {false};
+    float ypr[3];
+    float heading {0};
+
+    const float declinationAngle = 0.22;
+};

esp32/lib/ESP32-sveltekit/peripherals/peripherals.h

@@ -16,10 +16,10 @@
 #include <Wire.h>
 
 #include <Adafruit_BMP085_U.h>
-#include <Adafruit_HMC5883_U.h>
 #include <Adafruit_Sensor.h>
 #include <NewPing.h>
 #include <peripherals/imu.h>
+#include <peripherals/magnetometer.h>
 
 #define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
 
@@ -47,9 +47,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         : endpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this),
           _eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this,
                          EVENT_CONFIGURATION_SETTINGS),
-#if FT_ENABLED(USE_MAG)
-          _mag(12345),
-#endif
 #if FT_ENABLED(USE_BMP)
           _bmp(10085),
 #endif
@@ -78,10 +75,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
 #endif
 #if FT_ENABLED(USE_MAG)
-        mag_success = _mag.begin();
+        if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
-        if (!mag_success) {
-            ESP_LOGE("IMUService", "MAG initialize failed");
-        }
 #endif
 #if FT_ENABLED(USE_BMP)
         bmp_success = _bmp.begin();
@@ -159,20 +153,14 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         return updated;
     }
 
-    /* MAG FUNCTIONS */
+    bool readMag() {
-    float getHeading() {
+        bool updated = false;
-        float heading = 0;
 #if FT_ENABLED(USE_MAG)
-        sensors_event_t event;
+        beginTransaction();
-        _mag.getEvent(&event);
+        updated = _mag.readMagnetometer();
-        heading = atan2(event.magnetic.y, event.magnetic.x);
+        endTransaction();
-        float declinationAngle = 0.22;
-        heading += declinationAngle;
-        if (heading < 0) heading += 2 * PI;
-        if (heading > 2 * PI) heading -= 2 * PI;
-        heading *= 180 / M_PI;
 #endif
-        return heading;
+        return updated;
     }
 
     /* BMP FUNCTIONS */
@@ -226,9 +214,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         _imu.readIMU(root);
 #endif
 #if FT_ENABLED(USE_MAG)
-        if (mag_success) {
+        _mag.readMagnetometer(root);
-            doc["heading"] = round2(getHeading());
-        }
 #endif
 #if FT_ENABLED(USE_BMP)
         if (bmp_success) {
@@ -265,8 +251,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     IMU _imu;
 #endif
 #if FT_ENABLED(USE_MAG)
-    Adafruit_HMC5883_Unified _mag;
+    Magnetometer _mag;
-    bool mag_success {false};
 #endif
 #if FT_ENABLED(USE_BMP)
     Adafruit_BMP085_Unified _bmp;