Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

♻️ Moves peripherals to source file, add sensor base

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-11 18:34:08 +02:00
committed by Rune Harlyk
parent 41b863a0eb
commit a5371c36b9
7 changed files with 331 additions and 270 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/peripherals/barometer.h
+28 -3
View File
@@ -9,10 +9,35 @@
#include <Adafruit_BMP085_U.h>
#include <Adafruit_Sensor.h>
#include <peripherals/sensor.hpp>
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<JsonArray>();
arr.add(pressure);
arr.add(altitude);
arr.add(temperature);
arr.add(success);
}
void fromJson(JsonVariantConst v) override {
JsonArrayConst arr = v.as<JsonArrayConst>();
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};
esp32/include/peripherals/imu.h
+35 -49
View File
@@ -14,43 +14,41 @@
#include <Adafruit_BNO055.h>
#endif
struct IMUAnglesMsg {
#include <peripherals/sensor.hpp>
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<JsonArray>();
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<JsonArrayConst>();
rpy[0] = arr[0].as<float>();
rpy[1] = arr[1].as<float>();
rpy[2] = arr[2].as<float>();
temperature = arr[3].as<float>();
success = arr[4].as<bool>();
void fromJson(JsonVariantConst v) override {
JsonArrayConst arr = v.as<JsonArrayConst>();
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<IMUAnglesMsg> {
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;
};
esp32/include/peripherals/magnetometer.h
+21 -29
View File
@@ -9,40 +9,41 @@
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
struct MagnetometerMsg {
#include <peripherals/sensor.hpp>
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<JsonArray>();
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<JsonArrayConst>();
rpy[0] = arr[0].as<float>();
rpy[1] = arr[1].as<float>();
rpy[2] = arr[2].as<float>();
heading = arr[3].as<float>();
success = arr[4].as<bool>();
void fromJson(JsonVariantConst v) override {
JsonArrayConst arr = v.as<JsonArrayConst>();
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<MagnetometerMsg> {
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;
};
esp32/include/peripherals/peripherals.h
+19 -189
View File
@@ -36,216 +36,46 @@
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);
};
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<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
_right_sonar = std::make_unique<NewPing>(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<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();
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<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
#if FT_ENABLED(USE_MPU6050 || USE_BNO055) || FT_ENABLED(USE_HMC5883)
JsonVariant data = doc.as<JsonVariant>();
// socket.emit(EVENT_IMU, data);
#endif
}
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;
esp32/include/peripherals/sensor.hpp
+32
View File
@@ -0,0 +1,32 @@
#pragma once
#include <ArduinoJson.h>
struct SensorMessageBase {
bool success;
virtual void toJson(JsonVariant v) const = 0;
virtual void fromJson(JsonVariantConst v) = 0;
virtual ~SensorMessageBase() = default;
};
template <class T>
class SensorBase {
static_assert(std::is_base_of<SensorMessageBase, T>::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;
};
esp32/src/main.cpp
+11
View File
@@ -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<JsonVariant>();
peripherals.getI2CResult(results);
socket.emit(EVENT_I2C_SCAN, results);
});
}
void IRAM_ATTR SpotControlLoopEntry(void *) {
esp32/src/peripherals/peripherals.cpp
+185
View File
@@ -0,0 +1,185 @@
#include <peripherals/peripherals.h>
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<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
_right_sonar = std::make_unique<NewPing>(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<JsonArray>();
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<JsonArray>();
array[0] = _left_distance;
array[1] = _right_distance;
#endif
}