🚚 Moves firmware to src and include

esp32/include/peripherals/barometer.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <utils/math_utils.h>
+
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_Sensor.h>
+
+class Barometer {
+  public:
+    Barometer() : _bmp(10085) {}
+
+    bool initialize() {
+        bmp_success = _bmp.begin();
+        return bmp_success;
+    }
+
+    bool readBarometer() {
+        if (!bmp_success) return false;
+        _bmp.getTemperature(&temperature);
+        sensors_event_t event;
+        _bmp.getEvent(&event);
+        pressure = event.pressure;
+        altitude = _bmp.pressureToAltitude(seaLevelPressure, pressure);
+        return true;
+    }
+
+    float getPressure() { return pressure; }
+
+    float getAltitude() { return altitude; }
+
+    float getTemperature() { return temperature; }
+
+    void readBarometer(JsonObject& root) {
+        if (!bmp_success) return;
+        root["pressure"] = round2(getPressure());
+        root["altitude"] = round2(getAltitude());
+        root["bmp_temp"] = round2(getTemperature());
+    }
+
+    bool active() { return bmp_success; }
+
+  private:
+    Adafruit_BMP085_Unified _bmp;
+    bool bmp_success {false};
+    float pressure {0};
+    float altitude {0};
+    float temperature {0};
+
+    const float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+};

esp32/include/peripherals/camera_pins.h

@@ -0,0 +1,337 @@
+#if defined(CAMERA_MODEL_WROVER_KIT)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 21
+#define SIOD_GPIO_NUM 26
+#define SIOC_GPIO_NUM 27
+
+#define Y9_GPIO_NUM 35
+#define Y8_GPIO_NUM 34
+#define Y7_GPIO_NUM 39
+#define Y6_GPIO_NUM 36
+#define Y5_GPIO_NUM 19
+#define Y4_GPIO_NUM 18
+#define Y3_GPIO_NUM 5
+#define Y2_GPIO_NUM 4
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 23
+#define PCLK_GPIO_NUM 22
+
+#elif defined(CAMERA_MODEL_ESP_EYE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 4
+#define SIOD_GPIO_NUM 18
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 36
+#define Y8_GPIO_NUM 37
+#define Y7_GPIO_NUM 38
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 35
+#define Y4_GPIO_NUM 14
+#define Y3_GPIO_NUM 13
+#define Y2_GPIO_NUM 34
+#define VSYNC_GPIO_NUM 5
+#define HREF_GPIO_NUM 27
+#define PCLK_GPIO_NUM 25
+
+#define LED_GPIO_NUM 22
+
+#elif defined(CAMERA_MODEL_M5STACK_PSRAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_M5STACK_V2_PSRAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 22
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_M5STACK_WIDE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 22
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#define LED_GPIO_NUM 2
+
+#elif defined(CAMERA_MODEL_M5STACK_ESP32CAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 17
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_M5STACK_UNITCAM)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 32
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_AI_THINKER)
+#define PWDN_GPIO_NUM 32
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 0
+#define SIOD_GPIO_NUM 26
+#define SIOC_GPIO_NUM 27
+
+#define Y9_GPIO_NUM 35
+#define Y8_GPIO_NUM 34
+#define Y7_GPIO_NUM 39
+#define Y6_GPIO_NUM 36
+#define Y5_GPIO_NUM 21
+#define Y4_GPIO_NUM 19
+#define Y3_GPIO_NUM 18
+#define Y2_GPIO_NUM 5
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 23
+#define PCLK_GPIO_NUM 22
+
+// 4 for flash led or 33 for normal led
+#define LED_GPIO_NUM 4
+
+#elif defined(CAMERA_MODEL_TTGO_T_JOURNAL)
+#define PWDN_GPIO_NUM 0
+#define RESET_GPIO_NUM 15
+#define XCLK_GPIO_NUM 27
+#define SIOD_GPIO_NUM 25
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 19
+#define Y8_GPIO_NUM 36
+#define Y7_GPIO_NUM 18
+#define Y6_GPIO_NUM 39
+#define Y5_GPIO_NUM 5
+#define Y4_GPIO_NUM 34
+#define Y3_GPIO_NUM 35
+#define Y2_GPIO_NUM 17
+#define VSYNC_GPIO_NUM 22
+#define HREF_GPIO_NUM 26
+#define PCLK_GPIO_NUM 21
+
+#elif defined(CAMERA_MODEL_XIAO_ESP32S3)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 10
+#define SIOD_GPIO_NUM 40
+#define SIOC_GPIO_NUM 39
+
+#define Y9_GPIO_NUM 48
+#define Y8_GPIO_NUM 11
+#define Y7_GPIO_NUM 12
+#define Y6_GPIO_NUM 14
+#define Y5_GPIO_NUM 16
+#define Y4_GPIO_NUM 18
+#define Y3_GPIO_NUM 17
+#define Y2_GPIO_NUM 15
+#define VSYNC_GPIO_NUM 38
+#define HREF_GPIO_NUM 47
+#define PCLK_GPIO_NUM 13
+
+#elif defined(CAMERA_MODEL_ESP32_CAM_BOARD)
+// The 18 pin header on the board has Y5 and Y3 swapped
+#define USE_BOARD_HEADER 0
+#define PWDN_GPIO_NUM 32
+#define RESET_GPIO_NUM 33
+#define XCLK_GPIO_NUM 4
+#define SIOD_GPIO_NUM 18
+#define SIOC_GPIO_NUM 23
+
+#define Y9_GPIO_NUM 36
+#define Y8_GPIO_NUM 19
+#define Y7_GPIO_NUM 21
+#define Y6_GPIO_NUM 39
+#if USE_BOARD_HEADER
+#define Y5_GPIO_NUM 13
+#else
+#define Y5_GPIO_NUM 35
+#endif
+#define Y4_GPIO_NUM 14
+#if USE_BOARD_HEADER
+#define Y3_GPIO_NUM 35
+#else
+#define Y3_GPIO_NUM 13
+#endif
+#define Y2_GPIO_NUM 34
+#define VSYNC_GPIO_NUM 5
+#define HREF_GPIO_NUM 27
+#define PCLK_GPIO_NUM 25
+
+#elif defined(CAMERA_MODEL_ESP32S3_CAM_LCD)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 40
+#define SIOD_GPIO_NUM 17
+#define SIOC_GPIO_NUM 18
+
+#define Y9_GPIO_NUM 39
+#define Y8_GPIO_NUM 41
+#define Y7_GPIO_NUM 42
+#define Y6_GPIO_NUM 12
+#define Y5_GPIO_NUM 3
+#define Y4_GPIO_NUM 14
+#define Y3_GPIO_NUM 47
+#define Y2_GPIO_NUM 13
+#define VSYNC_GPIO_NUM 21
+#define HREF_GPIO_NUM 38
+#define PCLK_GPIO_NUM 11
+
+#elif defined(CAMERA_MODEL_ESP32S2_CAM_BOARD)
+// The 18 pin header on the board has Y5 and Y3 swapped
+#define USE_BOARD_HEADER 0
+#define PWDN_GPIO_NUM 1
+#define RESET_GPIO_NUM 2
+#define XCLK_GPIO_NUM 42
+#define SIOD_GPIO_NUM 41
+#define SIOC_GPIO_NUM 18
+
+#define Y9_GPIO_NUM 16
+#define Y8_GPIO_NUM 39
+#define Y7_GPIO_NUM 40
+#define Y6_GPIO_NUM 15
+#if USE_BOARD_HEADER
+#define Y5_GPIO_NUM 12
+#else
+#define Y5_GPIO_NUM 13
+#endif
+#define Y4_GPIO_NUM 5
+#if USE_BOARD_HEADER
+#define Y3_GPIO_NUM 13
+#else
+#define Y3_GPIO_NUM 12
+#endif
+#define Y2_GPIO_NUM 14
+#define VSYNC_GPIO_NUM 38
+#define HREF_GPIO_NUM 4
+#define PCLK_GPIO_NUM 3
+
+#elif defined(CAMERA_MODEL_ESP32S3_EYE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 15
+#define SIOD_GPIO_NUM 4
+#define SIOC_GPIO_NUM 5
+
+#define Y2_GPIO_NUM 11
+#define Y3_GPIO_NUM 9
+#define Y4_GPIO_NUM 8
+#define Y5_GPIO_NUM 10
+#define Y6_GPIO_NUM 12
+#define Y7_GPIO_NUM 18
+#define Y8_GPIO_NUM 17
+#define Y9_GPIO_NUM 16
+
+#define VSYNC_GPIO_NUM 6
+#define HREF_GPIO_NUM 7
+#define PCLK_GPIO_NUM 13
+
+#elif defined(CAMERA_MODEL_ESP32S3_WROVER)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 21
+#define SIOD_GPIO_NUM 26
+#define SIOC_GPIO_NUM 27
+
+#define Y2_GPIO_NUM 4
+#define Y3_GPIO_NUM 5
+#define Y4_GPIO_NUM 18
+#define Y5_GPIO_NUM 19
+#define Y6_GPIO_NUM 36
+#define Y7_GPIO_NUM 39
+#define Y8_GPIO_NUM 34
+#define Y9_GPIO_NUM 35
+
+#define VSYNC_GPIO_NUM 25
+#define HREF_GPIO_NUM 23
+#define PCLK_GPIO_NUM 22
+
+#elif defined(CAMERA_MODEL_ESP32S3_EYE)
+#define PWDN_GPIO_NUM -1
+#define RESET_GPIO_NUM -1
+#define XCLK_GPIO_NUM 15
+#define SIOD_GPIO_NUM 4
+#define SIOC_GPIO_NUM 5
+
+#define Y2_GPIO_NUM 11
+#define Y3_GPIO_NUM 9
+#define Y4_GPIO_NUM 8
+#define Y5_GPIO_NUM 10
+#define Y6_GPIO_NUM 12
+#define Y7_GPIO_NUM 18
+#define Y8_GPIO_NUM 17
+#define Y9_GPIO_NUM 16
+
+#define VSYNC_GPIO_NUM 6
+#define HREF_GPIO_NUM 7
+#define PCLK_GPIO_NUM 13
+
+#else
+#error "Camera model not selected"
+#endif

esp32/include/peripherals/camera_service.h

@@ -0,0 +1,51 @@
+#ifndef CameraService_h
+#define CameraService_h
+
+#include <ArduinoJson.h>
+#include <PsychicHttp.h>
+#include <WiFi.h>
+#include <async_worker.h>
+
+#include <features.h>
+#include <task_manager.h>
+#include <template/stateful_socket.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_endpoint.h>
+
+#include <settings/camera_settings.h>
+
+namespace Camera {
+
+#include <esp_camera.h>
+
+#if USE_CAMERA
+#include <peripherals/camera_pins.h>
+#endif
+
+#define PART_BOUNDARY "frame"
+
+#define EVENT_CAMERA_SETTINGS "CameraSettings"
+
+camera_fb_t *safe_camera_fb_get();
+sensor_t *safe_sensor_get();
+void safe_sensor_return();
+
+class CameraService : public StatefulService<CameraSettings> {
+  public:
+    CameraService();
+
+    esp_err_t begin();
+
+    esp_err_t cameraStill(PsychicRequest *request);
+    esp_err_t cameraStream(PsychicRequest *request);
+
+    StatefulHttpEndpoint<CameraSettings> endpoint;
+
+  private:
+    EventEndpoint<CameraSettings> _eventEndpoint;
+    FSPersistence<CameraSettings> _persistence;
+    void updateCamera();
+};
+} // namespace Camera
+
+#endif // end CameraService_h

esp32/include/peripherals/imu.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+#include <ArduinoJson.h>
+#include <utils/math_utils.h>
+
+#if FT_ENABLED(USE_MPU6050)
+#include <MPU6050_6Axis_MotionApps612.h>
+#endif
+
+#if FT_ENABLED(USE_BNO055)
+#include <Adafruit_BNO055.h>
+#endif
+
+class IMU {
+  public:
+    IMU()
+#if FT_ENABLED(USE_BNO055)
+        : _imu(55, 0x29)
+#endif
+    {
+    }
+    bool initialize() {
+#if FT_ENABLED(USE_MPU6050)
+        _imu.initialize();
+        imu_success = _imu.testConnection();
+        devStatus = _imu.dmpInitialize();
+        if (!imu_success) return false;
+        _imu.setDMPEnabled(true);
+        _imu.setI2CMasterModeEnabled(false);
+        _imu.setI2CBypassEnabled(true);
+        _imu.setSleepEnabled(false);
+#endif
+#if FT_ENABLED(USE_BNO055)
+        imu_success = _imu.begin();
+        if (!imu_success) {
+            return false;
+        }
+        _imu.setExtCrystalUse(true);
+#endif
+        return true;
+    }
+
+    bool readIMU() {
+        if (!imu_success) return false;
+#if FT_ENABLED(USE_MPU6050)
+        bool updated = _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
+        _imu.dmpGetQuaternion(&q, fifoBuffer);
+        _imu.dmpGetGravity(&gravity, &q);
+        _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        ypr[0] *= 180 / M_PI;
+        ypr[1] *= 180 / M_PI;
+        ypr[2] *= 180 / M_PI;
+        return updated;
+#endif
+#if FT_ENABLED(USE_BNO055)
+        sensors_event_t event;
+        _imu.getEvent(&event);
+        ypr[0] = (float)event.orientation.x;
+        ypr[1] = (float)event.orientation.y;
+        ypr[2] = (float)event.orientation.z;
+#endif
+        return true;
+    }
+
+    float getTemperature() { return imu_success ? imu_temperature : -1; }
+
+    float getAngleX() { return imu_success ? ypr[0] : 0; }
+
+    float getAngleY() { return imu_success ? ypr[1] : 0; }
+
+    float getAngleZ() { return imu_success ? ypr[2] : 0; }
+
+    void readIMU(JsonObject& root) {
+        if (!imu_success) return;
+        root["x"] = round2(getAngleX());
+        root["y"] = round2(getAngleY());
+        root["z"] = round2(getAngleZ());
+    }
+
+    bool active() { return imu_success; }
+
+  private:
+#if FT_ENABLED(USE_MPU6050)
+    MPU6050 _imu;
+    uint8_t devStatus {false};
+    Quaternion q;
+    uint8_t fifoBuffer[64];
+    VectorFloat gravity;
+#endif
+#if FT_ENABLED(USE_BNO055)
+    Adafruit_BNO055 _imu;
+#endif
+    bool imu_success {false};
+    float ypr[3];
+    float imu_temperature {-1};
+};

esp32/include/peripherals/led_service.h

@@ -0,0 +1,56 @@
+#ifndef LEDService_h
+#define LEDService_h
+
+#include <FastLED.h>
+
+#ifndef WS2812_PIN
+#define WS2812_PIN 12
+#endif
+
+#ifndef WS2812_NUM_LEDS
+#define WS2812_NUM_LEDS 1 + 12
+#endif
+
+#define COLOR_ORDER GRB
+#define CHIPSET WS2811
+
+class LEDService {
+  private:
+    CRGB leds[WS2812_NUM_LEDS];
+    CRGBPalette16 currentPalette;
+    TBlendType currentBlending;
+
+    int _brightness = 255;
+    int direction = 1;
+
+  public:
+    LEDService() {
+        FastLED.addLeds<CHIPSET, WS2812_PIN, COLOR_ORDER>(leds, WS2812_NUM_LEDS).setCorrection(TypicalLEDStrip);
+        currentPalette = OceanColors_p;
+        currentBlending = LINEARBLEND;
+    }
+    ~LEDService() {}
+
+    void loop() {
+        EXECUTE_EVERY_N_MS(1000 / 60, {
+            if (_brightness >= 200) direction = -5;
+            if (_brightness <= 50) direction = 5;
+            _brightness += direction;
+            if (WiFi.isConnected()) {
+                fillFromPallette(OceanColors_p, 0);
+            } else {
+                fillFromPallette(ForestColors_p, 128);
+            }
+            FastLED.show();
+        });
+    }
+
+    void fillFromPallette(CRGBPalette16 colorPalette, uint8_t colorIndex) {
+        CRGB color = ColorFromPalette(colorPalette, colorIndex, _brightness, currentBlending);
+        for (int i = 0; i < WS2812_NUM_LEDS; ++i) {
+            leds[i] = color;
+        }
+    }
+};
+
+#endif

esp32/include/peripherals/magnetometer.h

@@ -0,0 +1,59 @@
+#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() {
+        mag_success = _mag.begin();
+        return mag_success;
+    }
+
+    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/include/peripherals/peripherals.h

@@ -0,0 +1,239 @@
+#ifndef Peripherals_h
+#define Peripherals_h
+
+#include <template/stateful_socket.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_service.h>
+#include <utils/math_utils.h>
+#include <utils/timing.h>
+#include <filesystem.h>
+#include <features.h>
+#include <settings/peripherals_settings.h>
+#include <template/stateful_endpoint.h>
+
+#include <list>
+#include <SPI.h>
+#include <Wire.h>
+
+#include <NewPing.h>
+#include <peripherals/imu.h>
+#include <peripherals/magnetometer.h>
+#include <peripherals/barometer.h>
+
+#define EVENT_CONFIGURATION_SETTINGS "peripheralSettings"
+
+#define EVENT_I2C_SCAN "i2cScan"
+
+#define I2C_INTERVAL 250
+#define MAX_ESP_IMU_SIZE 500
+#define EVENT_IMU "imu"
+
+/*
+ * OLED Settings
+ */
+#define SCREEN_WIDTH 128
+#define SCREEN_HEIGHT 64
+#define SCREEN_RESET -1
+
+/*
+ * Ultrasonic Sensor Settings
+ */
+#define MAX_DISTANCE 200
+
+class Peripherals : public StatefulService<PeripheralsConfiguration> {
+  public:
+    Peripherals()
+        : endpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this),
+          _eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this,
+                         EVENT_CONFIGURATION_SETTINGS),
+          _persistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, DEVICE_CONFIG_FILE) {
+        _accessMutex = xSemaphoreCreateMutex();
+        addUpdateHandler([&](const String &originId) { updatePins(); }, false);
+    };
+
+    void begin() {
+        _eventEndpoint.begin();
+        _persistence.readFromFS();
+
+        socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) {
+            scanI2C();
+            emitI2C();
+        });
+
+        socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
+            scanI2C();
+            emitI2C(originId, sync);
+        });
+
+        updatePins();
+
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
+#endif
+#if FT_ENABLED(USE_HMC5883)
+        if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
+#endif
+#if FT_ENABLED(USE_BMP180)
+        if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
+#endif
+#if FT_ENABLED(USE_USS)
+        _left_sonar = new NewPing(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
+        _right_sonar = new NewPing(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
+#endif
+    };
+
+    void loop() {
+        EXECUTE_EVERY_N_MS(_updateInterval, {
+            beginTransaction();
+            emitIMU();
+            readSonar();
+            emitSonar();
+            endTransaction();
+        });
+    }
+
+    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);
+        }
+        ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync);
+        JsonVariant data = doc.as<JsonVariant>();
+        socket.emit(EVENT_I2C_SCAN, data, 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);
+    }
+
+    /* IMU FUNCTIONS */
+    bool readIMU() {
+        bool updated = false;
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        beginTransaction();
+        updated = _imu.readIMU();
+        endTransaction();
+#endif
+        return updated;
+    }
+
+    bool readMag() {
+        bool updated = false;
+#if FT_ENABLED(USE_HMC5883)
+        beginTransaction();
+        updated = _mag.readMagnetometer();
+        endTransaction();
+#endif
+        return updated;
+    }
+
+    bool readBMP() {
+        bool updated = false;
+#if FT_ENABLED(USE_BMP180)
+        beginTransaction();
+        updated = _bmp.readBarometer();
+        endTransaction();
+#endif
+        return updated;
+    }
+
+    void readSonar() {
+#if FT_ENABLED(USE_USS)
+        _left_distance = _left_sonar->ping_cm();
+        delay(50);
+        _right_distance = _right_sonar->ping_cm();
+#endif
+    }
+
+    float leftDistance() { return _left_distance; }
+    float rightDistance() { return _right_distance; }
+
+    StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
+
+    void emitIMU() {
+        doc.clear();
+        JsonObject root = doc.to<JsonObject>();
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        _imu.readIMU(root);
+#endif
+#if FT_ENABLED(USE_HMC5883)
+        _mag.readMagnetometer(root);
+#endif
+#if FT_ENABLED(USE_BMP180)
+        _bmp.readBarometer(root);
+#endif
+        JsonVariant data = doc.as<JsonVariant>();
+        socket.emit(EVENT_IMU, data);
+    }
+
+    void emitSonar() {
+#if FT_ENABLED(USE_USS)
+        doc.clear();
+        JsonArray root = doc.to<JsonArray>();
+        root[0] = _left_distance, root[1] = _right_distance;
+        JsonVariant data = doc.as<JsonVariant>();
+        socket.emit("sonar", data);
+#endif
+    }
+
+  private:
+    EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
+    FSPersistence<PeripheralsConfiguration> _persistence;
+
+    SemaphoreHandle_t _accessMutex;
+    inline void beginTransaction() { xSemaphoreTakeRecursive(_accessMutex, portMAX_DELAY); }
+
+    inline void endTransaction() { xSemaphoreGiveRecursive(_accessMutex); }
+
+    JsonDocument doc;
+    char message[MAX_ESP_IMU_SIZE];
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    IMU _imu;
+#endif
+#if FT_ENABLED(USE_HMC5883)
+    Magnetometer _mag;
+#endif
+#if FT_ENABLED(USE_BMP180)
+    Barometer _bmp;
+#endif
+#if FT_ENABLED(USE_USS)
+    NewPing *_left_sonar;
+    NewPing *_right_sonar;
+#endif
+    float _left_distance {MAX_DISTANCE};
+    float _right_distance {MAX_DISTANCE};
+
+    std::list<uint8_t> addressList;
+    bool i2c_active = false;
+    unsigned long _updateInterval {I2C_INTERVAL};
+};
+
+#endif

esp32/include/peripherals/servo_controller.h

@@ -0,0 +1,137 @@
+#ifndef ServoController_h
+#define ServoController_h
+
+#include <Adafruit_PWMServoDriver.h>
+#include <event_socket.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_service.h>
+#include <template/stateful_endpoint.h>
+#include <utils/math_utils.h>
+#include <settings/servo_settings.h>
+
+/*
+ * 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
+
+#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
+#define EVENT_SERVO_STATE "servoState"
+
+enum class SERVO_CONTROL_STATE { DEACTIVATED, PWM, ANGLE };
+
+class ServoController : public StatefulService<ServoSettings> {
+  public:
+    ServoController()
+        : endpoint(ServoSettings::read, ServoSettings::update, this),
+          _persistence(ServoSettings::read, ServoSettings::update, this, SERVO_SETTINGS_FILE) {}
+
+    void begin() {
+        socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
+                       [&](JsonVariant &root, int originId) { servoEvent(root, originId); });
+        socket.onEvent(EVENT_SERVO_STATE, [&](JsonVariant &root, int originId) { stateUpdate(root, originId); });
+        _persistence.readFromFS();
+
+        initializePCA();
+        socket.onEvent(EVENT_SERVO_STATE, [&](JsonVariant &root, int originId) {
+            is_active = root["active"] | false;
+            is_active ? activate() : deactivate();
+        });
+    }
+
+    void pcaWrite(int index, int value) {
+        if (value < 0 || value > 4096) {
+            ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
+            return;
+        }
+        _pca.setPWM(index, 0, value);
+    }
+
+    void activate() {
+        if (is_active) return;
+        control_state = SERVO_CONTROL_STATE::ANGLE;
+        is_active = true;
+        _pca.wakeup();
+    }
+
+    void deactivate() {
+        if (!is_active) return;
+        is_active = false;
+        control_state = SERVO_CONTROL_STATE::DEACTIVATED;
+        _pca.sleep();
+    }
+
+    void stateUpdate(JsonVariant &root, int originId) {
+        bool active = root["active"].as<bool>();
+        ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
+        active ? activate() : deactivate();
+    }
+
+    void servoEvent(JsonVariant &root, int originId) {
+        control_state = SERVO_CONTROL_STATE::PWM;
+        int8_t servo_id = root["servo_id"];
+        uint16_t pwm = root["pwm"].as<uint16_t>();
+        if (servo_id < 0) {
+            uint16_t pwms[12];
+            std::fill_n(pwms, 12, pwm);
+            _pca.setMultiplePWM(pwms, 12);
+        } else {
+            _pca.setPWM(servo_id, 0, pwm);
+        }
+        ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
+    }
+
+    void updateActiveState() { is_active ? activate() : deactivate(); }
+
+    void setAngles(float new_angles[12]) {
+        control_state = SERVO_CONTROL_STATE::ANGLE;
+        for (int i = 0; i < 12; i++) {
+            target_angles[i] = new_angles[i];
+        }
+    }
+
+    void calculatePWM() {
+        uint16_t pwms[12];
+        for (int i = 0; i < 12; i++) {
+            angles[i] = lerp(angles[i], target_angles[i], 0.05);
+            auto &servo = state().servos[i];
+            float angle = servo.direction * angles[i] + servo.centerAngle;
+            uint16_t pwm = angle * servo.conversion + servo.centerPwm;
+            if (pwm < 125 || pwm > 600) {
+                continue;
+            }
+            pwms[i] = pwm;
+        }
+        _pca.setMultiplePWM(pwms, 12);
+    }
+
+    void updateServoState() {
+        if (control_state == SERVO_CONTROL_STATE::ANGLE) calculatePWM();
+    }
+
+    StatefulHttpEndpoint<ServoSettings> endpoint;
+
+  private:
+    void initializePCA() {
+        _pca.begin();
+        _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
+        _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
+        _pca.sleep();
+    }
+    FSPersistence<ServoSettings> _persistence;
+
+    Adafruit_PWMServoDriver _pca;
+
+    SERVO_CONTROL_STATE control_state = SERVO_CONTROL_STATE::DEACTIVATED;
+
+    bool is_active {false};
+    float angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+    float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+};
+
+#endif