From a5371c36b9bc2737f5fc504039380538dcebdc28 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 11 Sep 2025 18:34:08 +0200 Subject: [PATCH] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Moves=20peripherals=20to?= =?UTF-8?q?=20source=20file,=20add=20sensor=20base?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/peripherals/barometer.h | 31 +++- esp32/include/peripherals/imu.h | 84 ++++----- esp32/include/peripherals/magnetometer.h | 50 +++--- esp32/include/peripherals/peripherals.h | 208 +++-------------------- esp32/include/peripherals/sensor.hpp | 32 ++++ esp32/src/main.cpp | 11 ++ esp32/src/peripherals/peripherals.cpp | 185 ++++++++++++++++++++ 7 files changed, 331 insertions(+), 270 deletions(-) create mode 100644 esp32/include/peripherals/sensor.hpp create mode 100644 esp32/src/peripherals/peripherals.cpp diff --git a/esp32/include/peripherals/barometer.h b/esp32/include/peripherals/barometer.h index e52973e..1950c65 100644 --- a/esp32/include/peripherals/barometer.h +++ b/esp32/include/peripherals/barometer.h @@ -9,10 +9,35 @@ #include #include +#include + +struct BarometerMsg : public SensorMessageBase { + float pressure {-1}; + float altitude {-1}; + float temperature {-1}; + bool success {false}; + + void toJson(JsonVariant v) const override { + JsonArray arr = v.to(); + arr.add(pressure); + arr.add(altitude); + arr.add(temperature); + arr.add(success); + } + + void fromJson(JsonVariantConst v) override { + JsonArrayConst arr = v.as(); + pressure = arr[0] | -1.0f; + altitude = arr[1] | -1.0f; + temperature = arr[2] | -1.0f; + success = arr[3] | false; + } + + friend void toJson(JsonVariant v, BarometerMsg const& a) { a.toJson(v); } +}; + class Barometer { public: - Barometer() : _bmp(10085) {} - bool initialize() { bmp_success = _bmp.begin(); return bmp_success; @@ -44,7 +69,7 @@ class Barometer { bool active() { return bmp_success; } private: - Adafruit_BMP085_Unified _bmp; + Adafruit_BMP085_Unified _bmp {10085}; bool bmp_success {false}; float pressure {0}; float altitude {0}; diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h index 853005d..7ea4c1d 100644 --- a/esp32/include/peripherals/imu.h +++ b/esp32/include/peripherals/imu.h @@ -14,43 +14,41 @@ #include #endif -struct IMUAnglesMsg { +#include + +struct IMUAnglesMsg : public SensorMessageBase { float rpy[3] {0, 0, 0}; float temperature {-1}; bool success {false}; - friend void toJson(JsonVariant v, IMUAnglesMsg const& a) { + void toJson(JsonVariant v) const override { JsonArray arr = v.to(); - arr.add(a.rpy[0]); - arr.add(a.rpy[1]); - arr.add(a.rpy[2]); - arr.add(a.temperature); - arr.add(a.success); + arr.add(rpy[0]); + arr.add(rpy[1]); + arr.add(rpy[2]); + arr.add(temperature); + arr.add(success); } - void fromJson(JsonVariantConst o) { - JsonArrayConst arr = o.as(); - rpy[0] = arr[0].as(); - rpy[1] = arr[1].as(); - rpy[2] = arr[2].as(); - temperature = arr[3].as(); - success = arr[4].as(); + void fromJson(JsonVariantConst v) override { + JsonArrayConst arr = v.as(); + rpy[0] = arr[0] | -1.0f; + rpy[1] = arr[1] | -1.0f; + rpy[2] = arr[2] | -1.0f; + temperature = arr[3] | -1.0f; + success = arr[4] | false; } + + friend void toJson(JsonVariant v, IMUAnglesMsg const& a) { a.toJson(v); } }; -class IMU { +class IMU : public SensorBase { public: - IMU() -#if FT_ENABLED(USE_BNO055) - : _imu(55, 0x29) -#endif - { - } - bool initialize() { + bool initialize() override { #if FT_ENABLED(USE_MPU6050) _imu.initialize(); - imuMsg.success = _imu.testConnection(); - if (!imuMsg.success) return false; + _msg.success = _imu.testConnection(); + if (!_msg.success) return false; devStatus = _imu.dmpInitialize(); if (devStatus == 0) { _imu.setDMPEnabled(false); @@ -64,8 +62,8 @@ class IMU { } #endif #if FT_ENABLED(USE_BNO055) - imuMsg.success = _imu.begin(); - if (!imuMsg.success) { + _msg.success = _imu.begin(); + if (!_msg.success) { return false; } _imu.setExtCrystalUse(true); @@ -73,14 +71,14 @@ class IMU { return true; } - bool readIMU() { - if (!imuMsg.success) return false; + bool update() override { + if (!_msg.success) return false; #if FT_ENABLED(USE_MPU6050) if (_imu.dmpPacketAvailable()) { if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) { _imu.dmpGetQuaternion(&q, fifoBuffer); _imu.dmpGetGravity(&gravity, &q); - _imu.dmpGetYawPitchRoll(imuMsg.rpy, &q, &gravity); + _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity); return true; } } @@ -89,31 +87,20 @@ class IMU { #if FT_ENABLED(USE_BNO055) sensors_event_t event; _imu.getEvent(&event); - imuMsg.rpy[0] = event.orientation.x; - imuMsg.rpy[1] = event.orientation.y; - imuMsg.rpy[2] = event.orientation.z; + _msg.rpy[0] = event.orientation.x; + _msg.rpy[1] = event.orientation.y; + _msg.rpy[2] = event.orientation.z; #endif return true; } - float getTemperature() { return imuMsg.temperature; } + float getTemperature() { return _msg.temperature; } - float getAngleX() { return imuMsg.rpy[2]; } + float getAngleX() { return _msg.rpy[2]; } - float getAngleY() { return imuMsg.rpy[1]; } + float getAngleY() { return _msg.rpy[1]; } - float getAngleZ() { return imuMsg.rpy[0]; } - - bool isActive() { return imuMsg.success; } - - IMUAnglesMsg getIMUAngles() { return imuMsg; } - - void readIMU(JsonObject& root) { - if (!imuMsg.success) return; - root["x"] = round2(getAngleX()); - root["y"] = round2(getAngleY()); - root["z"] = round2(getAngleZ()); - } + float getAngleZ() { return _msg.rpy[0]; } private: #if FT_ENABLED(USE_MPU6050) @@ -124,7 +111,6 @@ class IMU { VectorFloat gravity; #endif #if FT_ENABLED(USE_BNO055) - Adafruit_BNO055 _imu; + Adafruit_BNO055 _imu {55, 0x29}; #endif - IMUAnglesMsg imuMsg; }; \ No newline at end of file diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h index b3ddbd6..5877347 100644 --- a/esp32/include/peripherals/magnetometer.h +++ b/esp32/include/peripherals/magnetometer.h @@ -9,40 +9,41 @@ #include #include -struct MagnetometerMsg { +#include + +struct MagnetometerMsg : public SensorMessageBase { float rpy[3] {0, 0, 0}; float heading {-1}; - bool success {false}; - friend void toJson(JsonVariant v, MagnetometerMsg const& a) { + void toJson(JsonVariant v) const override { JsonArray arr = v.to(); - arr.add(a.rpy[0]); - arr.add(a.rpy[1]); - arr.add(a.rpy[2]); - arr.add(a.heading); - arr.add(a.success); + arr.add(rpy[0]); + arr.add(rpy[1]); + arr.add(rpy[2]); + arr.add(heading); + arr.add(success); } - void fromJson(JsonVariantConst o) { - JsonArrayConst arr = o.as(); - rpy[0] = arr[0].as(); - rpy[1] = arr[1].as(); - rpy[2] = arr[2].as(); - heading = arr[3].as(); - success = arr[4].as(); + void fromJson(JsonVariantConst v) override { + JsonArrayConst arr = v.as(); + rpy[0] = arr[0] | 0.0f; + rpy[1] = arr[1] | 0.0f; + rpy[2] = arr[2] | 0.0f; + heading = arr[3] | -1.0f; + success = arr[4] | false; } + + friend void toJson(JsonVariant v, MagnetometerMsg const& a) { a.toJson(v); } }; -class Magnetometer { +class Magnetometer : public SensorBase { public: - Magnetometer() : _mag(12345) {} - bool initialize() { msg.success = _mag.begin(); return msg.success; } - bool readMagnetometer() { + bool update() { if (!msg.success) return false; sensors_event_t event; bool updated = _mag.getEvent(&event); @@ -66,17 +67,8 @@ class Magnetometer { float getHeading() { return msg.heading; } - MagnetometerMsg getMagnetometerMsg() { return msg; } - - void readMagnetometer(JsonObject& root) { - if (!msg.success) return; - root["heading"] = round2(getHeading()); - } - - bool isActive() { return msg.success; } - private: - Adafruit_HMC5883_Unified _mag; + Adafruit_HMC5883_Unified _mag {12345}; MagnetometerMsg msg; const float declinationAngle = 0.22; }; \ No newline at end of file diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h index 547777b..9627a6f 100644 --- a/esp32/include/peripherals/peripherals.h +++ b/esp32/include/peripherals/peripherals.h @@ -36,216 +36,46 @@ class Peripherals : public StatefulService { 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); - }; + Peripherals(); - void begin() { - _eventEndpoint.begin(); - _persistence.readFromFS(); + void begin(); - // socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) { - // scanI2C(); - // emitI2C(); - // }); + void update(); - // socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) { - // scanI2C(); - // emitI2C(originId, sync); - // }); + void updatePins(); - updatePins(); + void scanI2C(uint8_t lower = 1, uint8_t higher = 127); -#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_PAJ7620U2) - if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed"); -#endif -#if FT_ENABLED(USE_USS) - _left_sonar = std::make_unique(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE); - _right_sonar = std::make_unique(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE); -#endif - }; + void getI2CResult(JsonVariant &root); - void update() { - readIMU(); - readMag(); - // _peripherals.readBMP(); - EXECUTE_EVERY_N_MS(100, { readGesture(); }); - } + void getIMUResult(JsonVariant &root); - 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(); - root["sda"] = state().sda; - root["scl"] = state().scl; - JsonArray addresses = root["addresses"].to(); - for (auto &address : addressList) { - addresses.add(address); - } - ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync); - JsonVariant data = doc.as(); - // 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(); - ESP_LOGI("Peripherals", "Scan complete - Found %d device(s)", nDevices); - } + void getSonarResult(JsonVariant &root); /* IMU FUNCTIONS */ - bool readIMU() { - bool updated = false; -#if FT_ENABLED(USE_MPU6050 || USE_BNO055) - beginTransaction(); - updated = _imu.readIMU(); - endTransaction(); -#endif - return updated; - } + bool readImu(); - bool readMag() { - bool updated = false; -#if FT_ENABLED(USE_HMC5883) - beginTransaction(); - updated = _mag.readMagnetometer(); - endTransaction(); -#endif - return updated; - } + bool readMag(); - bool readBMP() { - bool updated = false; -#if FT_ENABLED(USE_BMP180) - beginTransaction(); - updated = _bmp.readBarometer(); - endTransaction(); -#endif - return updated; - } + bool readBMP(); - bool readGesture() { - bool updated = false; -#if FT_ENABLED(USE_PAJ7620U2) - beginTransaction(); - updated = _gesture.readGesture(); - endTransaction(); -#endif - return updated; - } + bool readGesture(); - void readSonar() { -#if FT_ENABLED(USE_USS) - _left_distance = _left_sonar->ping_cm(); - delay(50); - _right_distance = _right_sonar->ping_cm(); -#endif - } + void readSonar(); - float angleX() { - return -#if FT_ENABLED(USE_MPU6050 || USE_BNO055) - _imu.getAngleX(); -#else - 0; -#endif - } + float angleX(); - float angleY() { - return -#if FT_ENABLED(USE_MPU6050 || USE_BNO055) - _imu.getAngleY(); -#else - 0; -#endif - } + float angleY(); - // float angleZ() { return _imu.getAngleZ(); } + float angleZ(); - gesture_t const takeGesture() { - return -#if FT_ENABLED(USE_PAJ7620U2) - _gesture.takeGesture(); -#else - gesture_t::eGestureNone; -#endif - } + gesture_t const takeGesture(); - float leftDistance() { return _left_distance; } - float rightDistance() { return _right_distance; } + float leftDistance(); + float rightDistance(); StatefulHttpEndpoint endpoint; - void emitIMU() { - doc.clear(); - JsonObject root = doc.to(); -#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 -#if FT_ENABLED(USE_MPU6050 || USE_BNO055) || FT_ENABLED(USE_HMC5883) - JsonVariant data = doc.as(); - // socket.emit(EVENT_IMU, data); -#endif - } - - void emitSonar() { -#if FT_ENABLED(USE_USS) - doc.clear(); - JsonArray root = doc.to(); - root[0] = _left_distance, root[1] = _right_distance; - JsonVariant data = doc.as(); - // socket.emit("sonar", data); -#endif - } - private: EventEndpoint _eventEndpoint; FSPersistence _persistence; diff --git a/esp32/include/peripherals/sensor.hpp b/esp32/include/peripherals/sensor.hpp new file mode 100644 index 0000000..a9ec8c6 --- /dev/null +++ b/esp32/include/peripherals/sensor.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include + +struct SensorMessageBase { + bool success; + virtual void toJson(JsonVariant v) const = 0; + virtual void fromJson(JsonVariantConst v) = 0; + + virtual ~SensorMessageBase() = default; +}; + +template +class SensorBase { + static_assert(std::is_base_of::value, "T must inherit from SensorMessageBase"); + + public: + SensorBase() {} + + virtual bool initialize() = 0; + + virtual bool update() = 0; + + virtual void getResults(JsonVariant &root) { _msg.toJson(root); } + + virtual T getResult() { return _msg; } + + virtual bool isActive() { return _msg.success; } + + protected: + T _msg; +}; \ No newline at end of file diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index 87236b1..7c7f78e 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -75,8 +75,10 @@ void setupServer() { #define INPUT_EVENT "input" #define MODE_EVENT "mode" #define WALK_GAIT_EVENT "walk_gait" +#define EVENT_I2C_SCAN "i2cScan" void setupEventSocket() { + // Motion events socket.onEvent(INPUT_EVENT, [&](JsonVariant &root, int originId) { motionService.handleInput(root, originId); }); socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) { motionService.handleMode(root, originId); }); @@ -85,6 +87,15 @@ void setupEventSocket() { [&](JsonVariant &root, int originId) { motionService.handleWalkGait(root, originId); }); socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { motionService.anglesEvent(root, originId); }); + + // Peripherals events + socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) { + peripherals.scanI2C(); + JsonDocument doc; + JsonVariant results = doc.to(); + peripherals.getI2CResult(results); + socket.emit(EVENT_I2C_SCAN, results); + }); } void IRAM_ATTR SpotControlLoopEntry(void *) { diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp new file mode 100644 index 0000000..59e780b --- /dev/null +++ b/esp32/src/peripherals/peripherals.cpp @@ -0,0 +1,185 @@ +#include + +Peripherals::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 Peripherals::begin() { + _eventEndpoint.begin(); + _persistence.readFromFS(); + + 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_PAJ7620U2) + if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed"); +#endif +#if FT_ENABLED(USE_USS) + _left_sonar = std::make_unique(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE); + _right_sonar = std::make_unique(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE); +#endif +}; + +void Peripherals::update() { + readImu(); + readMag(); + EXECUTE_EVERY_N_MS(100, { readGesture(); }); + EXECUTE_EVERY_N_MS(500, { readBMP(); }); + EXECUTE_EVERY_N_MS(500, { readSonar(); }); +} + +void Peripherals::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 Peripherals::getI2CResult(JsonVariant &root) { + char output[150]; + root["sda"] = state().sda; + root["scl"] = state().scl; + JsonArray addresses = root["addresses"].to(); + for (auto &address : addressList) { + addresses.add(address); + } + ESP_LOGI("Peripherals", "Emitting I2C scan results: %d", addressList.size()); +} + +void Peripherals::scanI2C(uint8_t lower, uint8_t higher) { + 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(); + ESP_LOGI("Peripherals", "Scan complete - Found %d device(s)", nDevices); +} + +/* IMU FUNCTIONS */ +bool Peripherals::readImu() { + bool updated = false; +#if FT_ENABLED(USE_MPU6050 || USE_BNO055) + beginTransaction(); + updated = _imu.update(); + endTransaction(); +#endif + return updated; +} + +bool Peripherals::readMag() { + bool updated = false; +#if FT_ENABLED(USE_HMC5883) + beginTransaction(); + updated = _mag.update(); + endTransaction(); +#endif + return updated; +} + +bool Peripherals::readBMP() { + bool updated = false; +#if FT_ENABLED(USE_BMP180) + beginTransaction(); + updated = _bmp.readBarometer(); + endTransaction(); +#endif + return updated; +} + +bool Peripherals::readGesture() { + bool updated = false; +#if FT_ENABLED(USE_PAJ7620U2) + beginTransaction(); + updated = _gesture.readGesture(); + endTransaction(); +#endif + return updated; +} + +void Peripherals::readSonar() { +#if FT_ENABLED(USE_USS) + _left_distance = _left_sonar->ping_cm(); + delay(50); + _right_distance = _right_sonar->ping_cm(); +#endif +} + +float Peripherals::angleX() { + return +#if FT_ENABLED(USE_MPU6050 || USE_BNO055) + _imu.getAngleX(); +#else + 0; +#endif +} + +float Peripherals::angleY() { + return +#if FT_ENABLED(USE_MPU6050 || USE_BNO055) + _imu.getAngleY(); +#else + 0; +#endif +} + +float Peripherals::angleZ() { + return +#if FT_ENABLED(USE_MPU6050 || USE_BNO055) + _imu.getAngleZ(); +#else + 0; +#endif +} + +gesture_t const Peripherals::takeGesture() { + return +#if FT_ENABLED(USE_PAJ7620U2) + _gesture.takeGesture(); +#else + gesture_t::eGestureNone; +#endif +} + +float Peripherals::leftDistance() { return _left_distance; } +float Peripherals::rightDistance() { return _right_distance; } + +void Peripherals::getIMUResult(JsonVariant &root) { +#if FT_ENABLED(USE_MPU6050 || USE_BNO055) + _imu.getResults(root); +#endif +#if FT_ENABLED(USE_HMC5883) + _mag.getResults(root); +#endif +#if FT_ENABLED(USE_BMP180) + _bmp.getResults(root); +#endif +} + +void Peripherals::getSonarResult(JsonVariant &root) { +#if FT_ENABLED(USE_USS) + JsonArray array = root.to(); + array[0] = _left_distance; + array[1] = _right_distance; +#endif +} \ No newline at end of file