From c400660a6fe8caafcf7e0ce8c3575c2d53674ba8 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Mon, 8 Jul 2024 21:20:06 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=96=B1=EF=B8=8F=20Combines=20deviceConfig?= =?UTF-8?q?=20and=20IMU=20service=20to=20peripherals?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/features.ini | 11 +- esp32/lib/ESP32-sveltekit/BatteryService.cpp | 12 +- esp32/lib/ESP32-sveltekit/BatteryService.h | 18 +- .../DeviceConfigurationService.h | 113 ----- esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp | 19 +- esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h | 19 +- esp32/lib/ESP32-sveltekit/IMUService.h | 196 -------- esp32/lib/ESP32-sveltekit/Peripherals.h | 424 ++++++++++++++++++ esp32/src/main.cpp | 2 +- 9 files changed, 463 insertions(+), 351 deletions(-) delete mode 100644 esp32/lib/ESP32-sveltekit/DeviceConfigurationService.h delete mode 100644 esp32/lib/ESP32-sveltekit/IMUService.h create mode 100644 esp32/lib/ESP32-sveltekit/Peripherals.h diff --git a/esp32/features.ini b/esp32/features.ini index 6082356..c476c13 100644 --- a/esp32/features.ini +++ b/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_MAG=0 - -D FT_BMP=0 + -D FT_IMU=1 + -D FT_MAG=1 + -D FT_BMP=1 -D FT_GPS=0 - -D FT_WS2812=0 + -D FT_WS2812=1 -D FT_SERVO=1 + -D FT_ADS1115=1 diff --git a/esp32/lib/ESP32-sveltekit/BatteryService.cpp b/esp32/lib/ESP32-sveltekit/BatteryService.cpp index 838b286..7ece921 100644 --- a/esp32/lib/ESP32-sveltekit/BatteryService.cpp +++ b/esp32/lib/ESP32-sveltekit/BatteryService.cpp @@ -14,16 +14,12 @@ #include -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; diff --git a/esp32/lib/ESP32-sveltekit/BatteryService.h b/esp32/lib/ESP32-sveltekit/BatteryService.h index bc0b843..27417e2 100644 --- a/esp32/lib/ESP32-sveltekit/BatteryService.h +++ b/esp32/lib/ESP32-sveltekit/BatteryService.h @@ -16,15 +16,27 @@ #include #include +#include + +#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; diff --git a/esp32/lib/ESP32-sveltekit/DeviceConfigurationService.h b/esp32/lib/ESP32-sveltekit/DeviceConfigurationService.h deleted file mode 100644 index cf9a3b3..0000000 --- a/esp32/lib/ESP32-sveltekit/DeviceConfigurationService.h +++ /dev/null @@ -1,113 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -#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 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 { - 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 _httpEndpoint; - EventEndpoint _eventEndpoint; - FSPersistence _fsPersistence; - - bool i2c_active = false; -}; diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp index 57d1692..8efd938 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp +++ b/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(); -#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP) - _imuService.begin(); -#endif + _peripherals.begin(); #if FT_ENABLED(FT_SERVO) _servoController.begin(); _servoController.configure(); @@ -230,18 +224,19 @@ 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) - _imuService.loop(); +#if FT_ENABLED(FT_BATTERY) + _batteryService.loop(); #endif #if FT_ENABLED(FT_MOTION) _motionService.loop(); #endif #if FT_ENABLED(FT_SERVO) - _servoController.loop(); + _servoController.loop(); #endif #if FT_ENABLED(FT_WS2812) _ledService.loop(); #endif + _peripherals.loop(); delay(20); } } diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h index f381da9..7676636 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h +++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -33,7 +33,6 @@ #include #include #include -#include #include #include #include @@ -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; -#if FT_ENABLED(FT_IMU) || FT_ENABLED(FT_MAG) || FT_ENABLED(FT_BMP) - IMUService _imuService; -#endif + Peripherals _peripherals; #if FT_ENABLED(FT_SERVO) ServoController _servoController; #endif diff --git a/esp32/lib/ESP32-sveltekit/IMUService.h b/esp32/lib/ESP32-sveltekit/IMUService.h deleted file mode 100644 index d78f9e0..0000000 --- a/esp32/lib/ESP32-sveltekit/IMUService.h +++ /dev/null @@ -1,196 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#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]; -}; diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h new file mode 100644 index 0000000..dafb93f --- /dev/null +++ b/esp32/lib/ESP32-sveltekit/Peripherals.h @@ -0,0 +1,424 @@ +#ifndef Peripherals_h +#define Peripherals_h + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#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 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 { + 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(); + root["sda"] = _state.sda; + root["scl"] = _state.scl; + JsonArray addresses = root["addresses"].to(); + 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 _httpEndpoint; + EventEndpoint _eventEndpoint; + FSPersistence _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 addressList; + bool i2c_active = false; + unsigned long _lastUpdate {0}; + unsigned long _updateInterval {I2C_INTERVAL}; +}; + +#endif \ No newline at end of file diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index 8bb20c9..d016dab 100644 --- a/esp32/src/main.cpp +++ b/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() {