⚡ Replace third party libs with i2c bus drivers
This commit is contained in:
Rune Harlyk
@@ -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;
|
||||
};
|
||||
@@ -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};
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
Reference in New Issue
Block a user