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

⚡ Replace third party libs with i2c bus drivers

Browse Source
This commit is contained in:
Rune Harlyk
2026-01-24 14:30:27 +01:00
committed by Rune Harlyk
parent d9e752777f
commit dbc74d6f88
16 changed files with 1005 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/peripherals/barometer.h
+12 -18
View File
@@ -1,13 +1,8 @@
#pragma once
#include <SPI.h>
#include <Wire.h>
#include <utils/math_utils.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_Sensor.h>
#include <peripherals/sensor.hpp>
#include <peripherals/drivers/bmp180.h>
struct BarometerMsg {
float pressure {-1};
@@ -20,29 +15,28 @@ class Barometer : public SensorBase<BarometerMsg> {
public:
bool initialize() override {
_msg.success = _bmp.begin();
if (_msg.success) {
ESP_LOGI("BMP", "BMP180 initialized successfully");
} else {
ESP_LOGE("BMP", "BMP180 initialization failed");
}
return _msg.success;
}
bool update() override {
if (!_msg.success) return false;
_bmp.getTemperature(&_msg.temperature);
sensors_event_t event;
_bmp.getEvent(&event);
_msg.pressure = event.pressure;
_msg.altitude = _bmp.pressureToAltitude(seaLevelPressure, _msg.pressure);
if (!_bmp.update()) return false;
_msg.temperature = _bmp.getTemperature();
_msg.pressure = _bmp.getPressure();
_msg.altitude = _bmp.getAltitude();
return true;
}
float getPressure() { return _msg.pressure; }
float getAltitude() { return _msg.altitude; }
float getTemperature() { return _msg.temperature; }
bool active() { return _msg.success; }
private:
Adafruit_BMP085_Unified _bmp {10085};
const float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
};
BMP180Driver _bmp;
};
esp32/include/peripherals/drivers/bmp180.h
+129
View File
@@ -0,0 +1,129 @@
#pragma once
#include <peripherals/i2c_bus.h>
#include <cmath>
class BMP180Driver {
public:
static constexpr uint8_t DEFAULT_ADDR = 0x77;
static constexpr float SEA_LEVEL_HPA = 1013.25f;
BMP180Driver(uint8_t addr = DEFAULT_ADDR) : _addr(addr) {}
bool begin() {
if (!I2CBus::instance().probe(_addr)) return false;
uint8_t id = readReg8(REG_CHIP_ID);
if (id != 0x55) return false;
_ac1 = readReg16(0xAA);
_ac2 = readReg16(0xAC);
_ac3 = readReg16(0xAE);
_ac4 = readReg16U(0xB0);
_ac5 = readReg16U(0xB2);
_ac6 = readReg16U(0xB4);
_b1 = readReg16(0xB6);
_b2 = readReg16(0xB8);
_mb = readReg16(0xBA);
_mc = readReg16(0xBC);
_md = readReg16(0xBE);
_initialized = true;
return true;
}
bool update() {
if (!_initialized) return false;
writeReg(REG_CONTROL, CMD_TEMP);
vTaskDelay(pdMS_TO_TICKS(5));
int32_t ut = readReg16(REG_OUT_MSB);
writeReg(REG_CONTROL, CMD_PRESSURE + (_oss << 6));
vTaskDelay(pdMS_TO_TICKS(2 + (3 << _oss)));
int32_t up = readReg24(REG_OUT_MSB) >> (8 - _oss);
int32_t x1 = ((ut - _ac6) * _ac5) >> 15;
int32_t x2 = (_mc << 11) / (x1 + _md);
int32_t b5 = x1 + x2;
_temperature = ((b5 + 8) >> 4) / 10.0f;
int32_t b6 = b5 - 4000;
x1 = (_b2 * ((b6 * b6) >> 12)) >> 11;
x2 = (_ac2 * b6) >> 11;
int32_t x3 = x1 + x2;
int32_t b3 = (((_ac1 * 4 + x3) << _oss) + 2) >> 2;
x1 = (_ac3 * b6) >> 13;
x2 = (_b1 * ((b6 * b6) >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
uint32_t b4 = (_ac4 * (uint32_t)(x3 + 32768)) >> 15;
uint32_t b7 = ((uint32_t)up - b3) * (50000 >> _oss);
int32_t p;
if (b7 < 0x80000000) {
p = (b7 << 1) / b4;
} else {
p = (b7 / b4) << 1;
}
x1 = (p >> 8) * (p >> 8);
x1 = (x1 * 3038) >> 16;
x2 = (-7357 * p) >> 16;
p = p + ((x1 + x2 + 3791) >> 4);
_pressure = p / 100.0f;
_altitude = 44330.0f * (1.0f - powf(_pressure / SEA_LEVEL_HPA, 0.1903f));
return true;
}
float getPressure() const { return _pressure; }
float getAltitude() const { return _altitude; }
float getTemperature() const { return _temperature; }
bool isInitialized() const { return _initialized; }
private:
static constexpr uint8_t REG_CHIP_ID = 0xD0;
static constexpr uint8_t REG_CONTROL = 0xF4;
static constexpr uint8_t REG_OUT_MSB = 0xF6;
static constexpr uint8_t CMD_TEMP = 0x2E;
static constexpr uint8_t CMD_PRESSURE = 0x34;
void writeReg(uint8_t reg, uint8_t val) { I2CBus::instance().writeReg(_addr, reg, &val, 1); }
uint8_t readReg8(uint8_t reg) {
uint8_t val = 0;
I2CBus::instance().readReg(_addr, reg, &val, 1);
return val;
}
int16_t readReg16(uint8_t reg) {
uint8_t buf[2];
I2CBus::instance().readReg(_addr, reg, buf, 2);
return (int16_t)((buf[0] << 8) | buf[1]);
}
uint16_t readReg16U(uint8_t reg) {
uint8_t buf[2];
I2CBus::instance().readReg(_addr, reg, buf, 2);
return (uint16_t)((buf[0] << 8) | buf[1]);
}
int32_t readReg24(uint8_t reg) {
uint8_t buf[3];
I2CBus::instance().readReg(_addr, reg, buf, 3);
return ((int32_t)buf[0] << 16) | ((int32_t)buf[1] << 8) | buf[2];
}
uint8_t _addr;
bool _initialized = false;
uint8_t _oss = 0;
int16_t _ac1, _ac2, _ac3;
uint16_t _ac4, _ac5, _ac6;
int16_t _b1, _b2;
int16_t _mb, _mc, _md;
float _temperature = 0;
float _pressure = 0;
float _altitude = 0;
};
esp32/include/peripherals/drivers/bno055.h
+111
View File
@@ -0,0 +1,111 @@
#pragma once
#include <peripherals/i2c_bus.h>
class BNO055Driver {
public:
static constexpr uint8_t DEFAULT_ADDR = 0x29;
BNO055Driver(uint8_t addr = DEFAULT_ADDR) : _addr(addr) {}
bool begin() {
if (!I2CBus::instance().probe(_addr)) return false;
uint8_t id = readReg(REG_CHIP_ID);
if (id != BNO055_ID) return false;
writeReg(REG_OPR_MODE, MODE_CONFIG);
vTaskDelay(pdMS_TO_TICKS(25));
writeReg(REG_SYS_TRIGGER, 0x20);
vTaskDelay(pdMS_TO_TICKS(650));
while (readReg(REG_CHIP_ID) != BNO055_ID) {
vTaskDelay(pdMS_TO_TICKS(10));
}
vTaskDelay(pdMS_TO_TICKS(50));
writeReg(REG_PWR_MODE, PWR_NORMAL);
vTaskDelay(pdMS_TO_TICKS(10));
writeReg(REG_PAGE_ID, 0);
writeReg(REG_SYS_TRIGGER, 0x80);
vTaskDelay(pdMS_TO_TICKS(10));
writeReg(REG_OPR_MODE, MODE_NDOF);
vTaskDelay(pdMS_TO_TICKS(20));
_initialized = true;
return true;
}
bool update() {
if (!_initialized) return false;
uint8_t buf[6];
if (I2CBus::instance().readReg(_addr, REG_EULER_H_LSB, buf, 6) != ESP_OK) return false;
int16_t h = (buf[1] << 8) | buf[0];
int16_t r = (buf[3] << 8) | buf[2];
int16_t p = (buf[5] << 8) | buf[4];
_euler[0] = h / 16.0f;
_euler[1] = r / 16.0f;
_euler[2] = p / 16.0f;
return true;
}
bool calibrate() {
if (!_initialized) return false;
uint8_t calData[22];
writeReg(REG_OPR_MODE, MODE_CONFIG);
vTaskDelay(pdMS_TO_TICKS(25));
if (I2CBus::instance().readReg(_addr, REG_ACCEL_OFFSET_X_LSB, calData, 22) != ESP_OK) {
writeReg(REG_OPR_MODE, MODE_NDOF);
return false;
}
writeReg(REG_OPR_MODE, MODE_NDOF);
vTaskDelay(pdMS_TO_TICKS(20));
return true;
}
float getHeading() const { return _euler[0]; }
float getRoll() const { return _euler[1]; }
float getPitch() const { return _euler[2]; }
bool isInitialized() const { return _initialized; }
uint8_t getCalibrationStatus() { return readReg(REG_CALIB_STAT); }
private:
static constexpr uint8_t BNO055_ID = 0xA0;
static constexpr uint8_t REG_CHIP_ID = 0x00;
static constexpr uint8_t REG_PAGE_ID = 0x07;
static constexpr uint8_t REG_ACCEL_OFFSET_X_LSB = 0x55;
static constexpr uint8_t REG_OPR_MODE = 0x3D;
static constexpr uint8_t REG_PWR_MODE = 0x3E;
static constexpr uint8_t REG_SYS_TRIGGER = 0x3F;
static constexpr uint8_t REG_EULER_H_LSB = 0x1A;
static constexpr uint8_t REG_CALIB_STAT = 0x35;
static constexpr uint8_t MODE_CONFIG = 0x00;
static constexpr uint8_t MODE_NDOF = 0x0C;
static constexpr uint8_t PWR_NORMAL = 0x00;
void writeReg(uint8_t reg, uint8_t val) { I2CBus::instance().writeReg(_addr, reg, &val, 1); }
uint8_t readReg(uint8_t reg) {
uint8_t val = 0;
I2CBus::instance().readReg(_addr, reg, &val, 1);
return val;
}
uint8_t _addr;
bool _initialized = false;
float _euler[3] = {0};
};
esp32/include/peripherals/drivers/hmc5883l.h
+81
View File
@@ -0,0 +1,81 @@
#pragma once
#include <peripherals/i2c_bus.h>
#include <cmath>
class HMC5883LDriver {
public:
static constexpr uint8_t DEFAULT_ADDR = 0x1E;
HMC5883LDriver(uint8_t addr = DEFAULT_ADDR) : _addr(addr) {}
bool begin() {
if (!I2CBus::instance().probe(_addr)) return false;
uint8_t idA = readReg(REG_ID_A);
uint8_t idB = readReg(REG_ID_B);
uint8_t idC = readReg(REG_ID_C);
if (idA != 'H' || idB != '4' || idC != '3') return false;
writeReg(REG_CONFIG_A, 0x70);
writeReg(REG_CONFIG_B, 0x20);
writeReg(REG_MODE, 0x00);
_initialized = true;
return true;
}
bool update() {
if (!_initialized) return false;
uint8_t buf[6];
if (I2CBus::instance().readReg(_addr, REG_DATA_X_MSB, buf, 6) != ESP_OK) return false;
int16_t x = (buf[0] << 8) | buf[1];
int16_t z = (buf[2] << 8) | buf[3];
int16_t y = (buf[4] << 8) | buf[5];
_mag[0] = x * _scale;
_mag[1] = y * _scale;
_mag[2] = z * _scale;
_heading = atan2f(_mag[1], _mag[0]);
_heading += _declination;
if (_heading < 0) _heading += 2 * M_PI;
if (_heading > 2 * M_PI) _heading -= 2 * M_PI;
_heading *= 180.0f / M_PI;
return true;
}
void setDeclination(float dec) { _declination = dec; }
float getMagX() const { return _mag[0]; }
float getMagY() const { return _mag[1]; }
float getMagZ() const { return _mag[2]; }
float getHeading() const { return _heading; }
bool isInitialized() const { return _initialized; }
private:
static constexpr uint8_t REG_CONFIG_A = 0x00;
static constexpr uint8_t REG_CONFIG_B = 0x01;
static constexpr uint8_t REG_MODE = 0x02;
static constexpr uint8_t REG_DATA_X_MSB = 0x03;
static constexpr uint8_t REG_ID_A = 0x0A;
static constexpr uint8_t REG_ID_B = 0x0B;
static constexpr uint8_t REG_ID_C = 0x0C;
static constexpr float _scale = 0.92f;
void writeReg(uint8_t reg, uint8_t val) { I2CBus::instance().writeReg(_addr, reg, &val, 1); }
uint8_t readReg(uint8_t reg) {
uint8_t val = 0;
I2CBus::instance().readReg(_addr, reg, &val, 1);
return val;
}
uint8_t _addr;
bool _initialized = false;
float _mag[3] = {0};
float _heading = 0;
float _declination = 0.22f;
};
esp32/include/peripherals/drivers/mpu6050.h
+232
View File
@@ -0,0 +1,232 @@
#pragma once
#include <peripherals/i2c_bus.h>
#include <utils/math_utils.h>
class MPU6050Driver {
public:
static constexpr uint8_t DEFAULT_ADDR = 0x68;
MPU6050Driver(uint8_t addr = DEFAULT_ADDR) : _addr(addr) {}
bool begin() {
if (!I2CBus::instance().probe(_addr)) return false;
writeReg(REG_PWR_MGMT_1, 0x80);
vTaskDelay(pdMS_TO_TICKS(100));
writeReg(REG_PWR_MGMT_1, 0x01);
writeReg(REG_PWR_MGMT_2, 0x00);
vTaskDelay(pdMS_TO_TICKS(50));
uint8_t who = readReg(REG_WHO_AM_I);
if (who != 0x68 && who != 0x98 && who != 0x72) return false;
setI2CMasterModeEnabled(false);
setI2CBypassEnabled(true);
setSleepEnabled(false);
writeReg(REG_SMPLRT_DIV, 4);
writeReg(REG_CONFIG, 0x03);
writeReg(REG_GYRO_CONFIG, 0x00);
writeReg(REG_ACCEL_CONFIG, 0x00);
writeReg(REG_INT_PIN_CFG, 0x02);
writeReg(REG_INT_ENABLE, 0x01);
resetFIFO();
setGyroOffsets(0, 0, 0);
setAccelOffsets(0, 0, 0);
_initialized = true;
return true;
}
bool update() {
if (!_initialized) return false;
uint8_t buf[14];
if (I2CBus::instance().readReg(_addr, REG_ACCEL_XOUT_H, buf, 14) != ESP_OK) return false;
int16_t rawAx = (buf[0] << 8) | buf[1];
int16_t rawAy = (buf[2] << 8) | buf[3];
int16_t rawAz = (buf[4] << 8) | buf[5];
int16_t rawTemp = (buf[6] << 8) | buf[7];
int16_t rawGx = (buf[8] << 8) | buf[9];
int16_t rawGy = (buf[10] << 8) | buf[11];
int16_t rawGz = (buf[12] << 8) | buf[13];
_accel[0] = (rawAx - _accelOffset[0]) / 16384.0f;
_accel[1] = (rawAy - _accelOffset[1]) / 16384.0f;
_accel[2] = (rawAz - _accelOffset[2]) / 16384.0f;
_gyro[0] = (rawGx - _gyroOffset[0]) / 131.0f;
_gyro[1] = (rawGy - _gyroOffset[1]) / 131.0f;
_gyro[2] = (rawGz - _gyroOffset[2]) / 131.0f;
_temp = rawTemp / 340.0f + 36.53f;
uint32_t now = xTaskGetTickCount() * portTICK_PERIOD_MS;
float dt = (now - _lastUpdate) / 1000.0f;
if (dt <= 0 || dt > 1.0f) dt = 0.02f;
_lastUpdate = now;
float accelMag = sqrtf(_accel[0] * _accel[0] + _accel[1] * _accel[1] + _accel[2] * _accel[2]);
bool accelValid = (accelMag > 0.8f && accelMag < 1.2f);
float accelRoll = atan2f(_accel[1], _accel[2]) * RAD2DEG_F;
float accelPitch = atan2f(-_accel[0], sqrtf(_accel[1] * _accel[1] + _accel[2] * _accel[2])) * RAD2DEG_F;
if (accelValid) {
_roll = ALPHA * (_roll + _gyro[0] * dt) + (1.0f - ALPHA) * accelRoll;
_pitch = ALPHA * (_pitch + _gyro[1] * dt) + (1.0f - ALPHA) * accelPitch;
} else {
_roll += _gyro[0] * dt;
_pitch += _gyro[1] * dt;
}
_yaw += _gyro[2] * dt;
return true;
}
bool calibrate() {
if (!_initialized) return false;
_gyroOffset[0] = 0;
_gyroOffset[1] = 0;
_gyroOffset[2] = 0;
_accelOffset[0] = 0;
_accelOffset[1] = 0;
_accelOffset[2] = 0;
int32_t gxSum = 0, gySum = 0, gzSum = 0;
int32_t axSum = 0, aySum = 0, azSum = 0;
int validSamples = 0;
for (int iteration = 0; iteration < 6; iteration++) {
gxSum = gySum = gzSum = 0;
axSum = aySum = azSum = 0;
validSamples = 0;
for (int i = 0; i < 100; i++) {
uint8_t buf[14];
if (I2CBus::instance().readReg(_addr, REG_ACCEL_XOUT_H, buf, 14) != ESP_OK) continue;
axSum += (int16_t)((buf[0] << 8) | buf[1]);
aySum += (int16_t)((buf[2] << 8) | buf[3]);
azSum += (int16_t)((buf[4] << 8) | buf[5]);
gxSum += (int16_t)((buf[8] << 8) | buf[9]);
gySum += (int16_t)((buf[10] << 8) | buf[11]);
gzSum += (int16_t)((buf[12] << 8) | buf[13]);
validSamples++;
vTaskDelay(pdMS_TO_TICKS(2));
}
if (validSamples == 0) return false;
_gyroOffset[0] += gxSum / validSamples;
_gyroOffset[1] += gySum / validSamples;
_gyroOffset[2] += gzSum / validSamples;
_accelOffset[0] += axSum / validSamples;
_accelOffset[1] += aySum / validSamples;
_accelOffset[2] += (azSum / validSamples) - 16384;
}
_roll = 0;
_pitch = 0;
_yaw = 0;
_lastUpdate = xTaskGetTickCount() * portTICK_PERIOD_MS;
return true;
}
void setGyroOffsets(int16_t x, int16_t y, int16_t z) {
uint8_t buf[6] = {static_cast<uint8_t>(x >> 8), static_cast<uint8_t>(x & 0xFF),
static_cast<uint8_t>(y >> 8), static_cast<uint8_t>(y & 0xFF),
static_cast<uint8_t>(z >> 8), static_cast<uint8_t>(z & 0xFF)};
I2CBus::instance().writeReg(_addr, REG_XG_OFFSET_H, buf, 6);
}
void setAccelOffsets(int16_t x, int16_t y, int16_t z) {
uint8_t buf[2];
buf[0] = x >> 8;
buf[1] = x & 0xFF;
I2CBus::instance().writeReg(_addr, REG_XA_OFFSET_H, buf, 2);
buf[0] = y >> 8;
buf[1] = y & 0xFF;
I2CBus::instance().writeReg(_addr, REG_YA_OFFSET_H, buf, 2);
buf[0] = z >> 8;
buf[1] = z & 0xFF;
I2CBus::instance().writeReg(_addr, REG_ZA_OFFSET_H, buf, 2);
}
void setI2CMasterModeEnabled(bool enabled) {
uint8_t val = readReg(REG_USER_CTRL);
writeReg(REG_USER_CTRL, enabled ? (val | 0x20) : (val & ~0x20));
}
void setI2CBypassEnabled(bool enabled) {
uint8_t val = readReg(REG_INT_PIN_CFG);
writeReg(REG_INT_PIN_CFG, enabled ? (val | 0x02) : (val & ~0x02));
}
void setSleepEnabled(bool enabled) {
uint8_t val = readReg(REG_PWR_MGMT_1);
writeReg(REG_PWR_MGMT_1, enabled ? (val | 0x40) : (val & ~0x40));
}
void resetFIFO() { writeReg(REG_USER_CTRL, readReg(REG_USER_CTRL) | 0x04); }
void setFIFOEnabled(bool enabled) {
uint8_t val = readReg(REG_USER_CTRL);
writeReg(REG_USER_CTRL, enabled ? (val | 0x40) : (val & ~0x40));
}
float getRoll() const { return _roll; }
float getPitch() const { return _pitch; }
float getYaw() const { return _yaw; }
float getTemperature() const { return _temp; }
const float* getAccel() const { return _accel; }
const float* getGyro() const { return _gyro; }
bool isInitialized() const { return _initialized; }
private:
static constexpr uint8_t REG_XG_OFFSET_H = 0x13;
static constexpr uint8_t REG_XA_OFFSET_H = 0x06;
static constexpr uint8_t REG_YA_OFFSET_H = 0x08;
static constexpr uint8_t REG_ZA_OFFSET_H = 0x0A;
static constexpr uint8_t REG_SMPLRT_DIV = 0x19;
static constexpr uint8_t REG_CONFIG = 0x1A;
static constexpr uint8_t REG_GYRO_CONFIG = 0x1B;
static constexpr uint8_t REG_ACCEL_CONFIG = 0x1C;
static constexpr uint8_t REG_FIFO_EN = 0x23;
static constexpr uint8_t REG_INT_PIN_CFG = 0x37;
static constexpr uint8_t REG_INT_ENABLE = 0x38;
static constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B;
static constexpr uint8_t REG_USER_CTRL = 0x6A;
static constexpr uint8_t REG_PWR_MGMT_1 = 0x6B;
static constexpr uint8_t REG_PWR_MGMT_2 = 0x6C;
static constexpr uint8_t REG_FIFO_COUNT_H = 0x72;
static constexpr uint8_t REG_FIFO_R_W = 0x74;
static constexpr uint8_t REG_WHO_AM_I = 0x75;
static constexpr float ALPHA = 0.98f;
void writeReg(uint8_t reg, uint8_t val) { I2CBus::instance().writeReg(_addr, reg, &val, 1); }
uint8_t readReg(uint8_t reg) {
uint8_t val = 0;
I2CBus::instance().readReg(_addr, reg, &val, 1);
return val;
}
uint8_t _addr;
bool _initialized = false;
float _accel[3] = {0};
float _gyro[3] = {0};
int16_t _gyroOffset[3] = {0};
int16_t _accelOffset[3] = {0};
float _temp = 0;
float _roll = 0;
float _pitch = 0;
float _yaw = 0;
uint32_t _lastUpdate = 0;
};
esp32/include/peripherals/drivers/pca9685.h
+119
View File
@@ -0,0 +1,119 @@
#pragma once
#include <peripherals/i2c_bus.h>
#include <algorithm>
class PCA9685Driver {
public:
static constexpr uint8_t DEFAULT_ADDR = 0x40;
PCA9685Driver(uint8_t addr = DEFAULT_ADDR) : _addr(addr) {}
bool begin() {
if (!I2CBus::instance().probe(_addr)) return false;
reset();
setOscillatorFrequency(25000000);
setPWMFreq(50);
_initialized = true;
return true;
}
void reset() {
writeReg(REG_MODE1, MODE1_RESTART);
vTaskDelay(pdMS_TO_TICKS(10));
}
void sleep() {
uint8_t mode = readReg(REG_MODE1);
writeReg(REG_MODE1, (mode & ~MODE1_RESTART) | MODE1_SLEEP);
vTaskDelay(pdMS_TO_TICKS(5));
}
void wakeup() {
uint8_t mode = readReg(REG_MODE1);
uint8_t wakeMode = mode & ~MODE1_SLEEP;
writeReg(REG_MODE1, wakeMode);
vTaskDelay(pdMS_TO_TICKS(5));
writeReg(REG_MODE1, wakeMode | MODE1_RESTART);
}
void setPWMFreq(float freq) {
freq = std::clamp(freq, 1.0f, 3500.0f);
float prescaleval = ((_oscFreq / (freq * 4096.0f)) + 0.5f) - 1;
if (prescaleval < 3) prescaleval = 3;
if (prescaleval > 255) prescaleval = 255;
uint8_t prescale = static_cast<uint8_t>(prescaleval);
uint8_t oldMode = readReg(REG_MODE1);
uint8_t newMode = (oldMode & ~MODE1_RESTART) | MODE1_SLEEP;
writeReg(REG_MODE1, newMode);
writeReg(REG_PRESCALE, prescale);
writeReg(REG_MODE1, oldMode);
vTaskDelay(pdMS_TO_TICKS(5));
writeReg(REG_MODE1, oldMode | MODE1_RESTART | MODE1_AI);
}
void setOscillatorFrequency(uint32_t freq) { _oscFreq = freq; }
uint8_t setPWM(uint8_t channel, uint16_t on, uint16_t off) {
if (channel > 15) return 1;
uint8_t buf[4] = {static_cast<uint8_t>(on & 0xFF), static_cast<uint8_t>(on >> 8),
static_cast<uint8_t>(off & 0xFF), static_cast<uint8_t>(off >> 8)};
return I2CBus::instance().writeReg(_addr, REG_LED0_ON_L + 4 * channel, buf, 4) == ESP_OK ? 0 : 1;
}
uint8_t setMultiplePWM(const uint16_t* values, uint8_t length) {
if (length > 16) length = 16;
uint8_t buf[64];
for (uint8_t i = 0; i < length; i++) {
uint16_t val = values[i] > 4095 ? 4095 : values[i];
uint8_t* b = &buf[i * 4];
b[0] = 0;
if (val == 0) {
b[1] = 0;
b[2] = 0;
b[3] = FULL_OFF_BIT;
} else if (val == 4095) {
b[1] = FULL_ON_BIT;
b[2] = 0;
b[3] = 0;
} else {
b[1] = 0;
b[2] = val & 0xFF;
b[3] = val >> 8;
}
}
return I2CBus::instance().writeReg(_addr, REG_LED0_ON_L, buf, length * 4) == ESP_OK ? 0 : 1;
}
bool isInitialized() const { return _initialized; }
private:
static constexpr uint8_t REG_MODE1 = 0x00;
static constexpr uint8_t REG_MODE2 = 0x01;
static constexpr uint8_t REG_PRESCALE = 0xFE;
static constexpr uint8_t REG_LED0_ON_L = 0x06;
static constexpr uint8_t MODE1_RESTART = 0x80;
static constexpr uint8_t MODE1_SLEEP = 0x10;
static constexpr uint8_t MODE1_AI = 0x20;
static constexpr uint8_t FULL_ON_BIT = 0x10;
static constexpr uint8_t FULL_OFF_BIT = 0x10;
void writeReg(uint8_t reg, uint8_t val) { I2CBus::instance().writeReg(_addr, reg, &val, 1); }
uint8_t readReg(uint8_t reg) {
uint8_t val = 0;
I2CBus::instance().readReg(_addr, reg, &val, 1);
return val;
}
uint8_t _addr;
bool _initialized = false;
uint32_t _oscFreq = 25000000;
};
esp32/include/peripherals/gesture.h
+5 -16
View File
@@ -1,6 +1,6 @@
#pragma once
#include <Wire.h>
#include <peripherals/i2c_bus.h>
enum gesture_t : uint16_t {
eGestureNone = 0x0000,
@@ -24,10 +24,9 @@ struct GestureMsg {
class PAJ7620U2 {
public:
PAJ7620U2(TwoWire* w = &Wire, uint8_t addr = 0x73) : wire(w), dev(addr) {}
PAJ7620U2(uint8_t addr = 0x73) : dev(addr) {}
int begin() {
uint16_t id = 0;
wire->begin();
selectBank(0x00);
if (readReg(REG_PART_ID, &id, 2) != 2) return ERR_BUS;
if (id != PART_ID) return ERR_IC;
@@ -74,21 +73,12 @@ class PAJ7620U2 {
void selectBank(uint8_t b) { writeReg(REG_BANK_SEL, &b, 1); }
void writeReg(uint8_t reg, const void* p, size_t n) {
if (!p || !n) return;
const uint8_t* d = static_cast<const uint8_t*>(p);
wire->beginTransmission(dev);
wire->write(reg);
for (size_t i = 0; i < n; i++) wire->write(d[i]);
wire->endTransmission();
I2CBus::instance().writeReg(dev, reg, static_cast<const uint8_t*>(p), n);
}
uint8_t readReg(uint8_t reg, void* p, size_t n) {
if (!p || !n) return 0;
uint8_t* d = static_cast<uint8_t*>(p);
wire->beginTransmission(dev);
wire->write(reg);
if (wire->endTransmission(false) != 0) return 0;
uint8_t r = wire->requestFrom(dev, static_cast<uint8_t>(n));
for (uint8_t i = 0; i < r; i++) d[i] = wire->read();
return r;
esp_err_t err = I2CBus::instance().readReg(dev, reg, static_cast<uint8_t*>(p), n);
return (err == ESP_OK) ? n : 0;
}
gesture_t mapGesture(gesture_t g) const {
@@ -118,7 +108,6 @@ class PAJ7620U2 {
return g;
}
TwoWire* wire;
uint8_t dev;
bool highRate = false;
orient_t orient = kRot180;
esp32/include/peripherals/i2c_bus.h
+160
View File
@@ -0,0 +1,160 @@
#pragma once
#include <driver/i2c.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <functional>
#include <vector>
class I2CBus {
public:
static I2CBus& instance() {
static I2CBus inst;
return inst;
}
esp_err_t begin(gpio_num_t sda, gpio_num_t scl, uint32_t freq = 100000, i2c_port_t port = I2C_NUM_0) {
if (_initialized) {
end();
}
_port = port;
_sda = sda;
_scl = scl;
_freq = freq;
i2c_config_t conf = {};
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = sda;
conf.scl_io_num = scl;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = freq;
esp_err_t err = i2c_param_config(_port, &conf);
if (err != ESP_OK) {
ESP_LOGE(TAG, "i2c_param_config failed: %s", esp_err_to_name(err));
return err;
}
err = i2c_driver_install(_port, I2C_MODE_MASTER, 0, 0, 0);
if (err == ESP_ERR_INVALID_STATE) {
ESP_LOGW(TAG, "I2C driver already installed for port %d, reconfiguring", _port);
i2c_driver_delete(_port);
err = i2c_param_config(_port, &conf);
if (err != ESP_OK) return err;
err = i2c_driver_install(_port, I2C_MODE_MASTER, 0, 0, 0);
}
if (err != ESP_OK) {
ESP_LOGE(TAG, "i2c_driver_install failed: %s", esp_err_to_name(err));
return err;
}
_initialized = true;
return ESP_OK;
}
void end() {
if (_initialized) {
i2c_driver_delete(_port);
_initialized = false;
}
}
bool isInitialized() const { return _initialized; }
esp_err_t writeBytes(uint8_t addr, const uint8_t* data, size_t len) {
if (!_initialized) return ESP_ERR_INVALID_STATE;
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
if (len > 0 && data != nullptr) {
i2c_master_write(cmd, data, len, true);
}
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(100));
i2c_cmd_link_delete(cmd);
return ret;
}
esp_err_t writeReg(uint8_t addr, uint8_t reg, const uint8_t* data, size_t len) {
if (!_initialized) return ESP_ERR_INVALID_STATE;
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
i2c_master_write_byte(cmd, reg, true);
if (len > 0 && data != nullptr) {
i2c_master_write(cmd, data, len, true);
}
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(100));
i2c_cmd_link_delete(cmd);
return ret;
}
esp_err_t readReg(uint8_t addr, uint8_t reg, uint8_t* data, size_t len) {
if (!_initialized) return ESP_ERR_INVALID_STATE;
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
i2c_master_write_byte(cmd, reg, true);
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_READ, true);
if (len > 1) {
i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
}
i2c_master_read_byte(cmd, data + len - 1, I2C_MASTER_NACK);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(100));
i2c_cmd_link_delete(cmd);
return ret;
}
bool probe(uint8_t addr) {
if (!_initialized) return false;
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(50));
i2c_cmd_link_delete(cmd);
return ret == ESP_OK;
}
std::vector<uint8_t> scan(uint8_t lower = 1, uint8_t upper = 127) {
std::vector<uint8_t> devices;
if (!_initialized) return devices;
for (uint8_t addr = lower; addr < upper; addr++) {
if (probe(addr)) {
devices.push_back(addr);
ESP_LOGI(TAG, "I2C device found at address 0x%02X", addr);
}
}
ESP_LOGI(TAG, "Scan complete - Found %d device(s)", devices.size());
return devices;
}
i2c_port_t port() const { return _port; }
gpio_num_t sda() const { return _sda; }
gpio_num_t scl() const { return _scl; }
uint32_t freq() const { return _freq; }
private:
I2CBus() = default;
~I2CBus() { end(); }
I2CBus(const I2CBus&) = delete;
I2CBus& operator=(const I2CBus&) = delete;
static constexpr const char* TAG = "I2CBus";
i2c_port_t _port = I2C_NUM_0;
gpio_num_t _sda = GPIO_NUM_NC;
gpio_num_t _scl = GPIO_NUM_NC;
uint32_t _freq = 100000;
bool _initialized = false;
};
esp32/include/peripherals/imu.h
+24 -68
View File
@@ -1,19 +1,17 @@
#pragma once
#include <SPI.h>
#include <Wire.h>
#include <utils/math_utils.h>
#include <features.h>
#include <peripherals/sensor.hpp>
#if FT_ENABLED(USE_MPU6050)
#include <MPU6050_6Axis_MotionApps612.h>
#include <peripherals/drivers/mpu6050.h>
#endif
#if FT_ENABLED(USE_BNO055)
#include <Adafruit_BNO055.h>
#include <peripherals/drivers/bno055.h>
#endif
#include <peripherals/sensor.hpp>
struct IMUAnglesMsg {
float rpy[3] {0, 0, 0};
float temperature {-1};
@@ -24,41 +22,20 @@ class IMU : public SensorBase<IMUAnglesMsg> {
public:
bool initialize() override {
#if FT_ENABLED(USE_MPU6050)
_imu.initialize();
_msg.success = _imu.testConnection();
_msg.success = _imu.begin();
if (!_msg.success) {
ESP_LOGE("IMU", "MPU6050 connection test failed");
ESP_LOGE("IMU", "MPU6050 initialization failed");
return false;
}
devStatus = _imu.dmpInitialize();
if (devStatus == 0) {
_imu.setXGyroOffset(0);
_imu.setYGyroOffset(0);
_imu.setZGyroOffset(0);
_imu.setXAccelOffset(0);
_imu.setYAccelOffset(0);
_imu.setZAccelOffset(0);
_imu.setI2CMasterModeEnabled(false);
_imu.setI2CBypassEnabled(true);
_imu.setSleepEnabled(false);
_imu.setRate(1);
_imu.resetFIFO();
_imu.setDMPEnabled(true);
ESP_LOGI("IMU", "MPU6050 DMP initialized successfully");
} else {
ESP_LOGE("IMU", "DMP initialization failed (code %d)", devStatus);
_msg.success = false;
}
ESP_LOGI("IMU", "MPU6050 initialized successfully");
#endif
#if FT_ENABLED(USE_BNO055)
_msg.success = _imu.begin();
if (!_msg.success) {
ESP_LOGE("IMU", "BNO055 connection test failed");
ESP_LOGE("IMU", "BNO055 initialization failed");
return false;
}
_imu.setExtCrystalUse(true);
ESP_LOGI("IMU", "BNO055 initialized successfully");
#endif
return _msg.success;
}
@@ -66,54 +43,37 @@ class IMU : public SensorBase<IMUAnglesMsg> {
bool update() override {
if (!_msg.success) return false;
#if FT_ENABLED(USE_MPU6050)
uint16_t fifoCount = _imu.getFIFOCount();
uint8_t intStatus = _imu.getIntStatus();
if (intStatus & 0x10) {
_imu.resetFIFO();
ESP_LOGW("IMU", "FIFO overflow, resetting");
return false;
}
if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
_imu.dmpGetQuaternion(&q, fifoBuffer);
_imu.dmpGetGravity(&gravity, &q);
_imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
return true;
}
return false;
if (!_imu.update()) return false;
_msg.rpy[0] = _imu.getYaw();
_msg.rpy[1] = _imu.getPitch();
_msg.rpy[2] = _imu.getRoll();
_msg.temperature = _imu.getTemperature();
#endif
#if FT_ENABLED(USE_BNO055)
sensors_event_t event;
_imu.getEvent(&event);
_msg.rpy[0] = event.orientation.x;
_msg.rpy[1] = event.orientation.y;
_msg.rpy[2] = event.orientation.z;
if (!_imu.update()) return false;
_msg.rpy[0] = _imu.getHeading();
_msg.rpy[1] = _imu.getPitch();
_msg.rpy[2] = _imu.getRoll();
#endif
return true;
}
float getTemperature() { return _msg.temperature; }
float getAngleX() { return _msg.rpy[2]; }
float getAngleY() { return _msg.rpy[1]; }
float getAngleZ() { return _msg.rpy[0]; }
bool calibrate() {
#if FT_ENABLED(USE_MPU6050)
if (!_msg.success) return false;
ESP_LOGI("IMU", "Starting calibration...");
_imu.CalibrateGyro(6);
_imu.CalibrateAccel(6);
bool result = _imu.calibrate();
ESP_LOGI("IMU", "Calibration complete");
return true;
return result;
#elif FT_ENABLED(USE_BNO055)
if (!_msg.success) return false;
ESP_LOGI("IMU", "Starting calibration...");
adafruit_bno055_offsets_t offsets;
bool result = _imu.getSensorOffsets(offsets);
bool result = _imu.calibrate();
ESP_LOGI("IMU", "Calibration complete");
return result;
#else
@@ -123,13 +83,9 @@ class IMU : public SensorBase<IMUAnglesMsg> {
private:
#if FT_ENABLED(USE_MPU6050)
MPU6050 _imu;
uint8_t devStatus {0};
Quaternion q;
uint8_t fifoBuffer[64];
VectorFloat gravity;
MPU6050Driver _imu;
#endif
#if FT_ENABLED(USE_BNO055)
Adafruit_BNO055 _imu {55, 0x29};
BNO055Driver _imu;
#endif
};
};
esp32/include/peripherals/magnetometer.h
+13 -23
View File
@@ -1,13 +1,8 @@
#pragma once
#include <SPI.h>
#include <Wire.h>
#include <utils/math_utils.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
#include <peripherals/sensor.hpp>
#include <peripherals/drivers/hmc5883l.h>
struct MagnetometerMsg {
float rpy[3] {0, 0, 0};
@@ -19,34 +14,29 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
public:
bool initialize() override {
_msg.success = _mag.begin();
if (_msg.success) {
ESP_LOGI("MAG", "HMC5883L initialized successfully");
} else {
ESP_LOGE("MAG", "HMC5883L initialization failed");
}
return _msg.success;
}
bool update() override {
if (!_msg.success) return false;
sensors_event_t event;
bool updated = _mag.getEvent(&event);
if (!updated) return false;
_msg.rpy[0] = event.magnetic.x;
_msg.rpy[1] = event.magnetic.y;
_msg.rpy[2] = event.magnetic.z;
_msg.heading = atan2(event.magnetic.y, event.magnetic.x);
_msg.heading += declinationAngle;
if (_msg.heading < 0) _msg.heading += 2 * PI;
if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
_msg.heading *= 180 / M_PI;
if (!_mag.update()) return false;
_msg.rpy[0] = _mag.getMagX();
_msg.rpy[1] = _mag.getMagY();
_msg.rpy[2] = _mag.getMagZ();
_msg.heading = _mag.getHeading();
return true;
}
float getMagX() { return _msg.rpy[0]; }
float getMagY() { return _msg.rpy[1]; }
float getMagZ() { return _msg.rpy[2]; }
float getHeading() { return _msg.heading; }
private:
Adafruit_HMC5883_Unified _mag {12345};
const float declinationAngle = 0.22;
};
HMC5883LDriver _mag;
};
esp32/include/peripherals/peripherals.h
+1 -1
View File
@@ -12,9 +12,9 @@
#include <list>
#include <SPI.h>
#include <Wire.h>
#include <NewPing.h>
#include <peripherals/i2c_bus.h>
#include <peripherals/imu.h>
#include <peripherals/magnetometer.h>
#include <peripherals/barometer.h>
esp32/include/peripherals/servo_controller.h
+4 -8
View File
@@ -1,16 +1,13 @@
#ifndef ServoController_h
#define ServoController_h
#include <Adafruit_PWMServoDriver.h>
#include <peripherals/drivers/pca9685.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
@@ -29,7 +26,6 @@ class ServoController : public StatefulService<ServoSettings> {
void begin() {
_persistence.readFromFS();
initializePCA();
}
@@ -98,13 +94,13 @@ class ServoController : public StatefulService<ServoSettings> {
private:
void initializePCA() {
_pca.begin();
_pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
_pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
_pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
_pca.sleep();
}
FSPersistence<ServoSettings> _persistence;
Adafruit_PWMServoDriver _pca;
PCA9685Driver _pca;
SERVO_CONTROL_STATE control_state = SERVO_CONTROL_STATE::DEACTIVATED;
@@ -113,4 +109,4 @@ class ServoController : public StatefulService<ServoSettings> {
float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
};
#endif
#endif