🖱️ Combines deviceConfig and IMU service to peripherals

esp32/features.ini

@@ -1,6 +1,6 @@
 [features]
 build_flags = 
-  -D FT_BATTERY=0
+  -D FT_BATTERY=1
   -D FT_NTP=1
   -D FT_SECURITY=0
   -D FT_SLEEP=0
@@ -10,9 +10,10 @@ build_flags =
   -D FT_MOTION=1
 
   ; Hardware specific
-  -D FT_IMU=0
+  -D FT_IMU=1
-  -D FT_MAG=0
+  -D FT_MAG=1
-  -D FT_BMP=0  
+  -D FT_BMP=1
   -D FT_GPS=0
-  -D FT_WS2812=0
+  -D FT_WS2812=1
   -D FT_SERVO=1
+  -D FT_ADS1115=1

esp32/lib/ESP32-sveltekit/BatteryService.cpp

@@ -14,16 +14,12 @@
 
 #include <BatteryService.h>
 
-BatteryService::BatteryService(EventSocket *socket) : _socket(socket)
+BatteryService::BatteryService(Peripherals *peripherals, EventSocket *socket)
-{
+    : _peripherals(peripherals), _socket(socket) {}
-}
 
-void BatteryService::begin()
+void BatteryService::begin() {}
-{
-}
 
-void BatteryService::batteryEvent()
+void BatteryService::batteryEvent() {
-{
     JsonDocument doc;
     char message[64];
     doc["voltage"] = _voltage;

esp32/lib/ESP32-sveltekit/BatteryService.h

@@ -16,15 +16,27 @@
 
 #include <EventSocket.h>
 #include <JsonUtils.h>
+#include <Peripherals.h>
+
+#define ADC_VOLTAGE 0
+#define ADC_CURRENT 1
+#define ADC_BUTTON 2
 
 #define EVENT_BATTERY "battery"
 #define BATTERY_INTERVAL 10000
 #define BATTERY_CHECK_INTERVAL 1000
 
+// #define CURRENT_FACTOR 0.185 // 5A
+// #define CURRENT_FACTOR 0.100 // 20A
+#define CURRENT_FACTOR 0.066 // 30A
+
+#define VOLTAGE_THRESHOLD 6.4
+#define CURRENT_THRESHOLD 5
+
 class BatteryService
 {
 public:
-    BatteryService(EventSocket *socket);
+    BatteryService(Peripherals *peripherals, EventSocket *socket);
 
     void begin();
 
@@ -45,6 +57,9 @@ public:
 
     void updateBattery()
     {
+        _voltage = _peripherals->readADCVoltage(ADC_VOLTAGE);
+        float voltage = _peripherals->readADCVoltage(ADC_CURRENT);
+        _current = (voltage - 2.5) / CURRENT_FACTOR;
     }
 
     float getVoltage() {
@@ -58,6 +73,7 @@ public:
 private:
     void batteryEvent();
     EventSocket *_socket;
+    Peripherals *_peripherals;
 
     unsigned long _lastUpdate;
     unsigned long _lastEmit;

esp32/lib/ESP32-sveltekit/DeviceConfigurationService.h

@@ -1,113 +0,0 @@
-#pragma once
-
-#include <EventEndpoint.h>
-#include <FSPersistence.h>
-#include <HttpEndpoint.h>
-#include <SecurityManager.h>
-#include <StatefulService.h>
-
-#include <SPI.h>
-#include <Wire.h>
-
-#define DEVICE_CONFIG_FILE "/config/deviceConfig.json"
-#define EVENT_CONFIGURATION_SETTINGS "ConfigurationSettings"
-#define CONFIGURATION_SETTINGS_PATH "/api/configuration"
-
-/*
- * OLED Settings
- */
-#define SCREEN_WIDTH 128
-#define SCREEN_HEIGHT 64
-#define SCREEN_RESET -1
-
-/*
- * I2C software connection 
- */
-#ifndef SDA_PIN
-    #define SDA_PIN 14
-#endif
-#ifndef SCL_PIN
-    #define SCL_PIN 15
-#endif
-
-/*
- * Ultra sonic sensors
- */
-#define USS_LEFT 12
-#define USS_RIGHT 13
-#define USS_MAX_DISTANCE 200
-
-class PinConfig {
-public:
-    int pin;
-    String mode;
-    String type;
-    String role;
-
-    PinConfig(int p, String m, String t, String r) : pin(p), mode(m), type(t), role(r) {}
-};
-
-class DeviceConfiguration {
-   public:
-    int sda = SDA_PIN;
-    int scl = SCL_PIN;
-    std::vector<PinConfig> pins;
-
-    static void read(DeviceConfiguration &settings, JsonObject &root) {
-        root["sda"] = settings.sda;
-        root["scl"] = settings.scl;
-    }
-
-    static StateUpdateResult update(JsonObject &root, DeviceConfiguration &settings) {
-        settings.sda = root["sda"] | SDA_PIN;
-        settings.scl = root["scl"] | SCL_PIN;
-        return StateUpdateResult::CHANGED;
-    };
-};
-
-class DeviceConfigurationService : public StatefulService<DeviceConfiguration> {
-   public:
-    DeviceConfigurationService(PsychicHttpServer *server, FS *fs,
-                               SecurityManager *securityManager,
-                               EventSocket *socket)
-        : _server(server),
-          _securityManager(securityManager),
-          _httpEndpoint(DeviceConfiguration::read, DeviceConfiguration::update,
-                        this, server, CONFIGURATION_SETTINGS_PATH,
-                        securityManager, AuthenticationPredicates::IS_ADMIN),
-          _eventEndpoint(DeviceConfiguration::read, DeviceConfiguration::update,
-                         this, socket, EVENT_CONFIGURATION_SETTINGS),
-          _fsPersistence(DeviceConfiguration::read, DeviceConfiguration::update,
-                         this, fs, DEVICE_CONFIG_FILE)
-                         {
-                            addUpdateHandler([&](const String &originId) { updatePins(); },
-                         false);
-                         };
-
-    void begin() {
-        _httpEndpoint.begin();
-        _eventEndpoint.begin();
-        _fsPersistence.readFromFS();
-        updatePins();
-    };
-
-    void updatePins() {
-        if (i2c_active){
-            Wire.end();
-        }
-
-        if (_state.sda != -1 && _state.scl != -1) {
-            Wire.begin(_state.sda, _state.scl);
-            i2c_active = true;
-        }
-    }
-
-   private:
-    PsychicHttpServer *_server;
-    SecurityManager *_securityManager;
-    HttpEndpoint<DeviceConfiguration> _httpEndpoint;
-    EventEndpoint<DeviceConfiguration> _eventEndpoint;
-    FSPersistence<DeviceConfiguration> _fsPersistence;
-
-    bool i2c_active = false;
-};

esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp

@@ -47,7 +47,7 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server,
       _sleepService(server, &_securitySettingsService),
 #endif
 #if FT_ENABLED(FT_BATTERY)
-      _batteryService(&_socket),
+      _batteryService(&_peripherals, &_socket),
 #endif
 #if FT_ENABLED(FT_ANALYTICS)
       _analyticsService(&_socket, &_taskManager),
@@ -63,16 +63,13 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server,
 #if FT_ENABLED(FT_MOTION)
       _motionService(_server, &_socket, &_securitySettingsService,&_taskManager),
 #endif
-#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP)
-      _imuService(&_socket),
-#endif
 #if FT_ENABLED(FT_WS2812)
         _ledService(&_taskManager),
 #endif
 #if FT_ENABLED(FT_SERVO)
       _servoController(server, &ESPFS, &_securitySettingsService, &_socket),
 #endif
-      _deviceConfiguration(server, &ESPFS, &_securitySettingsService, &_socket)
+      _peripherals(server, &ESPFS, &_securitySettingsService, &_socket)
 { }
 
 void ESP32SvelteKit::begin() {
@@ -210,10 +207,7 @@ void ESP32SvelteKit::startServices() {
     _cameraService.begin();
     _cameraSettingsService.begin();
 #endif
-    _deviceConfiguration.begin();
+    _peripherals.begin();
-#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP)
-        _imuService.begin();
-#endif
 #if FT_ENABLED(FT_SERVO)
     _servoController.begin();
     _servoController.configure();
@@ -230,8 +224,8 @@ void IRAM_ATTR ESP32SvelteKit::_loop() {
 #if FT_ENABLED(FT_ANALYTICS)
         _analyticsService.loop();
 #endif
-#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP)
+#if FT_ENABLED(FT_BATTERY)
-        _imuService.loop();
+        _batteryService.loop();
 #endif
 #if FT_ENABLED(FT_MOTION)
         _motionService.loop();
@@ -242,6 +236,7 @@ void IRAM_ATTR ESP32SvelteKit::_loop() {
 #if FT_ENABLED(FT_WS2812)
         _ledService.loop();
 #endif
+    _peripherals.loop();
         delay(20);
     }
 }

esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h

@@ -25,7 +25,7 @@
 #include <BatteryService.h>
 #include <FileExplorerService.h>
 #include <DownloadFirmwareService.h>
-#include <DeviceConfigurationService.h>
+#include <Peripherals.h>
 #include <ServoController.h>
 #include <ESPFS.h>
 #include <ESPmDNS.h>
@@ -33,7 +33,6 @@
 #include <EventSocket.h>
 #include <FactoryResetService.h>
 #include <FeaturesService.h>
-#include <IMUService.h>
 #include <MotionService.h>
 #include <NTPSettingsService.h>
 #include <CameraService.h>
@@ -164,18 +163,11 @@ public:
     }
 #endif
 
-    DeviceConfigurationService *getDeviceConfigurationService()
+    Peripherals *getPeripherals()
     {
-        return &_deviceConfiguration;
+        return &_peripherals;
     }
 
-#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP)
-    IMUService *getIMUService()
-    {
-        return &_imuService;
-    }
-#endif
-
 #if FT_ENABLED(FT_SERVO)
     ServoController *getServoController()
     {
@@ -243,10 +235,7 @@ private:
     CameraService _cameraService;
     CameraSettingsService _cameraSettingsService;
 #endif
-    DeviceConfigurationService _deviceConfiguration;
+    Peripherals _peripherals;
-#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP)
-    IMUService _imuService;
-#endif
 #if FT_ENABLED(FT_SERVO)
     ServoController _servoController;
 #endif

esp32/lib/ESP32-sveltekit/IMUService.h

@@ -1,196 +0,0 @@
-#pragma once
-
-#include <Adafruit_BMP085_U.h>
-#include <Adafruit_HMC5883_U.h>
-#include <Adafruit_Sensor.h>
-#include <ArduinoJson.h>
-#include <EventSocket.h>
-#include <MPU6050_6Axis_MotionApps612.h>
-
-#define IMU_INTERVAL 500
-#define MAX_ESP_IMU_SIZE 500
-#define EVENT_IMU "imu"
-
-class IMUService
-{
-public:
-    IMUService(EventSocket *socket) 
-    : 
-#if FT_ENABLED(FT_MAG)
-    _mag(12345),
-#endif
-#if FT_ENABLED(FT_BMP)
-    _bmp(10085),
-#endif
-    _socket(socket)
-    {};
-
-    void begin()
-    {
-#if FT_ENABLED(FT_IMU)
-        _imu.initialize();
-        imu_success = _imu.testConnection();
-        devStatus = _imu.dmpInitialize();
-        if(!imu_success) {
-            ESP_LOGE("IMUService", "MPU initialize failed");
-        }
-        _imu.setDMPEnabled(true);
-        _imu.setI2CMasterModeEnabled(false);
-        _imu.setI2CBypassEnabled(true);
-        _imu.setSleepEnabled(false);
-#endif
-#if FT_ENABLED(FT_MAG)
-        mag_success = _mag.begin();
-        if(!mag_success) {
-            ESP_LOGE("IMUService", "MAG initialize failed");
-        }
-#endif
-#if FT_ENABLED(FT_BMP)
-        bmp_success = _bmp.begin();
-        if(!bmp_success) {
-            ESP_LOGE("IMUService", "BMP initialize failed");
-        }
-#endif
-    };
-
-#if FT_ENABLED(FT_IMU)
-    bool isIMUSuccess() {
-        return imu_success;
-    }
-
-    float getTemp() {
-        return imu_success ? imu_temperature : -1;
-    }
-
-    float getAngleX() {
-        return imu_success ? ypr[0] * 180/M_PI : -1;
-    }
-
-    float getAngleY() {
-        return imu_success ? ypr[1] * 180/M_PI : -1;
-    }
-
-    float getAngleZ() {
-        return imu_success ? ypr[2] * 180/M_PI : -1;
-    }
-
-    bool readIMU() {
-        bool updated = imu_success && _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
-        _imu.dmpGetQuaternion(&q, fifoBuffer);
-        _imu.dmpGetGravity(&gravity, &q);
-        _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-        return updated;
-    }
-#endif
-
-#if FT_ENABLED(FT_MAG)
-    float getHeading() {
-        sensors_event_t event; 
-        _mag.getEvent(&event);
-        float heading = atan2(event.magnetic.y, event.magnetic.x);
-        float declinationAngle = 0.22;
-        heading += declinationAngle;
-        if(heading < 0) heading += 2 * PI;
-        if(heading > 2 * PI) heading -= 2 * PI;
-        return heading * 180/M_PI;
-    }
-#endif
-
-#if FT_ENABLED(FT_BMP)
-    bool isBMPSuccess() {
-        return bmp_success;
-    }
-
-    float getAltitude() {
-        sensors_event_t event;
-        _bmp.getEvent(&event);
-        float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
-        return bmp_success && event.pressure ? _bmp.pressureToAltitude(seaLevelPressure, event.pressure) : -1;
-    }
-
-    float getPressure() {
-        sensors_event_t event;
-        _bmp.getEvent(&event);
-        return bmp_success && event.pressure ? event.pressure : -1;
-    }
-
-    float getTemperature() {
-        float temperature;
-        _bmp.getTemperature(&temperature);
-        return bmp_success ? temperature : -1;
-    }
-#endif
-
-    double round2(double value) {
-        return (int)(value * 100 + 0.5) / 100.0;
-    }
-
-    void loop()
-    {
-        unsigned long currentMillis = millis();
-
-        if (currentMillis - _lastUpdate >= _updateInterval)
-        {
-            _lastUpdate = currentMillis;
-            updateImu();
-        }
-    };
-    
-protected:
-    JsonDocument doc;
-
-    void updateImu() {
-        doc.clear();
-        bool newData = false;
-#if FT_ENABLED(FT_IMU)
-        newData = imu_success && readIMU();
-        if (imu_success) {
-            doc["x"] = round2(getAngleX());
-            doc["y"] = round2(getAngleY());
-            doc["z"] = round2(getAngleZ());
-        }
-#endif
-#if FT_ENABLED(FT_MAG)
-        newData = newData || mag_success;
-        if (mag_success) {
-            doc["heading"] = round2(getHeading());
-        }
-#endif
-#if FT_ENABLED(FT_BMP)
-        newData = newData || bmp_success;
-        if (bmp_success) {
-            doc["pressure"] = round2(getPressure());
-            doc["altitude"] = round2(getAltitude());
-            doc["bmp_temp"] = round2(getTemperature());
-        }
-#endif
-        if(newData) {
-            serializeJson(doc, message);
-            _socket->emit(EVENT_IMU, message);
-        }
-    }
-
-private:
-#if FT_ENABLED(FT_IMU)
-    MPU6050 _imu;
-    bool imu_success {false};
-    uint8_t devStatus {false};
-    Quaternion q;
-    uint8_t fifoBuffer[64];
-    VectorFloat gravity;
-    float ypr[3];
-    float imu_temperature {-1};
-#endif
-#if FT_ENABLED(FT_MAG)
-    Adafruit_HMC5883_Unified _mag;
-    bool mag_success {false};
-#endif
-#if FT_ENABLED(FT_BMP)
-    Adafruit_BMP085_Unified _bmp; 
-    bool bmp_success {false};
-#endif
-    EventSocket *_socket;
-    unsigned long _lastUpdate {0};
-    unsigned long _updateInterval {IMU_INTERVAL};
-    char message[MAX_ESP_IMU_SIZE];
-};

esp32/lib/ESP32-sveltekit/Peripherals.h

@@ -0,0 +1,424 @@
+#ifndef Peripherals_h
+#define Peripherals_h
+
+#include <EventEndpoint.h>
+#include <FSPersistence.h>
+#include <HttpEndpoint.h>
+#include <SecurityManager.h>
+#include <StatefulService.h>
+#include <list>
+
+#include <SPI.h>
+#include <Wire.h>
+
+#include <MPU6050_6Axis_MotionApps612.h>
+#include <Adafruit_PWMServoDriver.h>
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_HMC5883_U.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_ADS1X15.h>
+
+#define DEVICE_CONFIG_FILE "/config/peripheral.json"
+#define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
+#define CONFIGURATION_SETTINGS_PATH "/api/peripheral/settings"
+
+#define EVENT_I2C_SCAN "i2cScan"
+
+#define I2C_INTERVAL 500
+#define MAX_ESP_IMU_SIZE 500
+#define EVENT_IMU "imu"
+
+
+/*
+ * Servo Settings
+ */
+#ifndef FACTORY_SERVO_PWM_FREQUENCY
+#define FACTORY_SERVO_PWM_FREQUENCY 50
+#endif
+
+#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
+#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
+#endif
+
+/*
+ * OLED Settings
+ */
+#define SCREEN_WIDTH 128
+#define SCREEN_HEIGHT 64
+#define SCREEN_RESET -1
+
+/*
+ * I2C software connection 
+ */
+#ifndef SDA_PIN
+#define SDA_PIN SDA
+#endif
+#ifndef SCL_PIN
+#define SCL_PIN SCL
+#endif
+#ifndef I2C_FREQUENCY
+#define I2C_FREQUENCY 100000UL
+#endif
+
+class PinConfig {
+public:
+    int pin;
+    String mode;
+    String type;
+    String role;
+
+    PinConfig(int p, String m, String t, String r) : pin(p), mode(m), type(t), role(r) {}
+};
+
+class PeripheralsConfiguration {
+   public:
+    int sda = SDA_PIN;
+    int scl = SCL_PIN;
+    long frequency = I2C_FREQUENCY;
+    std::vector<PinConfig> pins;
+
+    static void read(PeripheralsConfiguration &settings, JsonObject &root) {
+        root["sda"] = settings.sda;
+        root["scl"] = settings.scl;
+        root["frequency"] = settings.frequency;
+    }
+
+    static StateUpdateResult update(JsonObject &root, PeripheralsConfiguration &settings) {
+        settings.sda = root["sda"] | SDA_PIN;
+        settings.scl = root["scl"] | SCL_PIN;
+        settings.frequency = root["frequency"] | I2C_FREQUENCY;
+        return StateUpdateResult::CHANGED;
+    };
+};
+
+class Peripherals : public StatefulService<PeripheralsConfiguration> {
+   public:
+    Peripherals(PsychicHttpServer *server, FS *fs,
+                               SecurityManager *securityManager,
+                               EventSocket *socket)
+        : _server(server),
+          _socket(socket),
+          _securityManager(securityManager),
+          _httpEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update,
+                        this, server, CONFIGURATION_SETTINGS_PATH,
+                        securityManager, AuthenticationPredicates::IS_ADMIN),
+          _eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update,
+                         this, socket, EVENT_CONFIGURATION_SETTINGS),
+            #if FT_ENABLED(FT_MAG)
+                _mag(12345),
+            #endif
+            #if FT_ENABLED(FT_BMP)
+                _bmp(10085),
+            #endif
+          _fsPersistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update,
+                         this, fs, DEVICE_CONFIG_FILE)
+                         {
+                            addUpdateHandler([&](const String &originId) { updatePins(); },
+                         false);
+                         };
+
+    void begin() {
+        _httpEndpoint.begin();
+        _eventEndpoint.begin();
+        _fsPersistence.readFromFS();
+
+        _socket->onEvent(EVENT_I2C_SCAN, [&](JsonObject &root, int originId) {
+            scanI2C();
+            emitI2C();
+        });
+
+        _socket->onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
+            scanI2C();
+            emitI2C(originId, sync);
+        });
+
+        updatePins();
+
+        #if FT_ENABLED(FT_SERVO)
+        _pca.begin();
+        _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
+        _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
+        #endif
+
+        #if FT_ENABLED(FT_IMU)
+        _imu.initialize();
+        imu_success = _imu.testConnection();
+        devStatus = _imu.dmpInitialize();
+        if(!imu_success) {
+            ESP_LOGE("IMUService", "MPU initialize failed");
+        }
+        _imu.setDMPEnabled(true);
+        _imu.setI2CMasterModeEnabled(false);
+        _imu.setI2CBypassEnabled(true);
+        _imu.setSleepEnabled(false);
+        #endif
+        #if FT_ENABLED(FT_MAG)
+        mag_success = _mag.begin();
+        if(!mag_success) {
+            ESP_LOGE("IMUService", "MAG initialize failed");
+                }
+        #endif
+        #if FT_ENABLED(FT_BMP)
+        bmp_success = _bmp.begin();
+        if(!bmp_success) {
+            ESP_LOGE("IMUService", "BMP initialize failed");
+        }
+        #endif
+
+        #if FT_ENABLED(FT_ADS1015) || FT_ENABLED(FT_ADS1115)
+        if (!_ads.begin()) {
+            ESP_LOGE("Peripherals", "ADS1015/ADS1115 not found");
+        }
+        _ads.startADCReading(ADS1X15_REG_CONFIG_MUX_DIFF_0_1, /*continuous=*/false);
+        #endif
+    };
+
+    void loop() {
+        unsigned long currentMillis = millis();
+
+        if (currentMillis - _lastUpdate >= _updateInterval)
+        {
+            _lastUpdate = currentMillis;
+            readIMU();
+        }
+    }
+
+    void updatePins() {
+        if (i2c_active){
+            Wire.end();
+        }
+
+        if (_state.sda != -1 && _state.scl != -1) {
+            Wire.begin(_state.sda, _state.scl, _state.frequency);
+            i2c_active = true;
+        }
+    }
+
+    void emitI2C(const String &originId = "", bool sync = false) {
+        char output[150];
+        JsonDocument doc;
+        JsonObject root = doc.to<JsonObject>();
+        root["sda"] = _state.sda;
+        root["scl"] = _state.scl;
+        JsonArray addresses = root["addresses"].to<JsonArray>();
+        for (auto &address : addressList) {
+            addresses.add(address);
+        }
+        serializeJson(root, output);
+        ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync);
+        _socket->emit(EVENT_I2C_SCAN, output, originId.c_str(), sync);
+    }
+
+    void scanI2C(uint8_t lower = 1, uint8_t higher = 127) {
+        addressList.clear();
+        for (uint8_t address = lower; address < higher; address++) {
+            Wire.beginTransmission(address);
+            if (Wire.endTransmission() == 0) {
+                addressList.emplace_back(address);
+                ESP_LOGI("Peripherals", "I2C device found at address 0x%02X", address);
+            }
+        }
+        uint8_t nDevices = addressList.size();
+        if (nDevices == 0)
+            ESP_LOGI("Peripherals", "No I2C devices found");
+        else
+            ESP_LOGI("Peripherals", "Scan complete - Found %d devices", nDevices);
+    }
+
+    /* SERVO FUNCTIONS*/
+    void pcaWrite(int index, int value) {
+    #if FT_ENABLED(FT_SERVO)
+        _pca.setPWM(index, 0, value);
+    #endif
+    }
+
+    void pcaAngle(int index, int value) {
+    #if FT_ENABLED(FT_SERVO)
+        // uint16_t pwm = SERVO_MIN + value * SERVO_ANGLE_PWM_RATIO;
+        _pca.setPWM(index, 0, 125 + value * 2);
+    #endif
+    }
+
+    /* ADC FUNCTIONS*/
+    int16_t readADCVoltage(uint8_t channel) {
+        int16_t voltage = -1;
+    #if FT_ENABLED(FT_ADS1015) || FT_ENABLED(FT_ADS1115)
+        float adc0 = _ads.readADC_SingleEnded(channel);
+        voltage = _ads.computeVolts(adc0);
+    #endif
+        return voltage;
+    }
+
+    /* IMU FUNCTIONS */
+    bool readIMU() {
+        bool updated = false;
+        #if FT_ENABLED(FT_IMU)
+        updated = imu_success && _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
+        _imu.dmpGetQuaternion(&q, fifoBuffer);
+        _imu.dmpGetGravity(&gravity, &q);
+        _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        #endif
+        return updated;
+    }
+
+    float getTemp() {
+        float temp = -1;
+        #if FT_ENABLED(FT_IMU)
+        temp = imu_success ? imu_temperature : -1;
+        #endif
+        return temp;
+    }
+
+    float getAngleX() {
+        float angle = 0;
+        #if FT_ENABLED(FT_IMU)
+        angle = imu_success ? ypr[0] * 180/M_PI : 0;
+        #endif
+        return angle;
+    }
+
+    float getAngleY() {
+        float angle = 0;
+        #if FT_ENABLED(FT_IMU)
+        angle = imu_success ? ypr[1] * 180/M_PI : 0;
+        #endif
+        return angle;
+    }
+
+    float getAngleZ() {
+        float angle = 0;
+        #if FT_ENABLED(FT_IMU)
+        angle = imu_success ? ypr[2] * 180/M_PI : 0;
+        #endif
+        return angle;
+    }
+
+    /* MAG FUNCTIONS */
+    float getHeading() {
+        float heading = 0;
+        #if FT_ENABLED(FT_MAG)
+        sensors_event_t event; 
+        _mag.getEvent(&event);
+        heading = atan2(event.magnetic.y, event.magnetic.x);
+        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;
+    }
+
+    /* BMP FUNCTIONS */
+    float getAltitude() {
+        float altitude = -1;
+        #if FT_ENABLED(FT_MAG)
+        sensors_event_t event;
+        _bmp.getEvent(&event);
+        float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+        altitude = bmp_success && event.pressure ? _bmp.pressureToAltitude(seaLevelPressure, event.pressure) : -1;
+        #endif
+        return altitude;
+    }
+
+    float getPressure() {
+        float pressure = -1;
+        #if FT_ENABLED(FT_BMP)
+        sensors_event_t event;
+        _bmp.getEvent(&event);
+        pressure = bmp_success && event.pressure ? event.pressure : -1;
+        #endif
+        return pressure;
+    }
+
+    float getTemperature() {
+        float temperature = 0;
+        #if FT_ENABLED(FT_BMP)
+        _bmp.getTemperature(&temperature);
+        temperature = bmp_success ? temperature : -1;
+        #endif
+        return temperature;
+    }
+
+    double round2(double value) {
+        return (int)(value * 100 + 0.5) / 100.0;
+    }
+
+protected:
+    void updateImu() {
+        doc.clear();
+        bool newData = false;
+        #if FT_ENABLED(FT_IMU)
+        newData = imu_success && readIMU();
+        if (imu_success) {
+            doc["x"] = round2(getAngleX());
+            doc["y"] = round2(getAngleY());
+            doc["z"] = round2(getAngleZ());
+        }
+        #endif
+        #if FT_ENABLED(FT_MAG)
+        newData = newData || mag_success;
+        if (mag_success) {
+            doc["heading"] = round2(getHeading());
+        }
+        #endif
+        #if FT_ENABLED(FT_BMP)
+        newData = newData || bmp_success;
+        if (bmp_success) {
+            doc["pressure"] = round2(getPressure());
+            doc["altitude"] = round2(getAltitude());
+            doc["bmp_temp"] = round2(getTemperature());
+        }
+        #endif
+        if(newData) {
+            serializeJson(doc, message);
+            _socket->emit(EVENT_IMU, message);
+        }
+    }
+
+   private:
+    PsychicHttpServer *_server;
+    EventSocket *_socket;
+    SecurityManager *_securityManager;
+    HttpEndpoint<PeripheralsConfiguration> _httpEndpoint;
+    EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
+    FSPersistence<PeripheralsConfiguration> _fsPersistence;
+
+    JsonDocument doc;
+    char message[MAX_ESP_IMU_SIZE];
+
+#if FT_ENABLED(FT_SERVO)
+    Adafruit_PWMServoDriver _pca;
+#endif
+#if FT_ENABLED(FT_IMU)
+    MPU6050 _imu;
+    bool imu_success {false};
+    uint8_t devStatus {false};
+    Quaternion q;
+    uint8_t fifoBuffer[64];
+    VectorFloat gravity;
+    float ypr[3];
+    float imu_temperature {-1};
+#endif
+#if FT_ENABLED(FT_MAG)
+    Adafruit_HMC5883_Unified _mag;
+    bool mag_success {false};
+#endif
+#if FT_ENABLED(FT_BMP)
+    Adafruit_BMP085_Unified _bmp; 
+    bool bmp_success {false};
+#endif
+#if FT_ENABLED(FT_ADS1015)
+    Adafruit_ADS1015 _ads;
+#endif
+#if FT_ENABLED(FT_ADS1115)
+    Adafruit_ADS1115 _ads;
+#endif
+
+    std::list<uint8_t> addressList;
+    bool i2c_active = false;
+    unsigned long _lastUpdate {0};
+    unsigned long _updateInterval {I2C_INTERVAL};
+};
+
+#endif

esp32/src/main.cpp

@@ -5,7 +5,7 @@
 
 DRAM_ATTR PsychicHttpServer server;
 
-DRAM_ATTR ESP32SvelteKit spot(&server, 126);
+DRAM_ATTR ESP32SvelteKit spot(&server, 130);
 
 void setup()
 {