diff --git a/app/src/routes/peripherals/imu/imu.svelte b/app/src/routes/peripherals/imu/imu.svelte index 665be3b..1d5d2db 100644 --- a/app/src/routes/peripherals/imu/imu.svelte +++ b/app/src/routes/peripherals/imu/imu.svelte @@ -66,114 +66,120 @@ const colors = getChartColors() const baseConfig = createBaseChartConfig(colors.background) - angleChart = new Chart(angleChartElement, { - type: 'line', - data: { - datasets: [ - { - label: 'x', - borderColor: colors.primary, - backgroundColor: colors.primary, - borderWidth: 2, - data: $imu.map(datapoint => datapoint.x), - yAxisID: 'y' - }, - { - label: 'y', - borderColor: colors.secondary, - backgroundColor: colors.secondary, - borderWidth: 2, - data: $imu.map(datapoint => datapoint.y), - yAxisID: 'y' - }, - { - label: 'z', - borderColor: colors.accent, - backgroundColor: colors.accent, - borderWidth: 2, - data: $imu.map(datapoint => datapoint.z), - yAxisID: 'y' - } - ] - }, - options: { - ...baseConfig, - scales: { - ...baseConfig.scales, - y: { - ...baseConfig.scales.y, - title: { - display: true, - text: 'Angle [°]', - color: colors.background, - font: { size: 16, weight: 'bold' } + if (angleChartElement) { + angleChart = new Chart(angleChartElement, { + type: 'line', + data: { + datasets: [ + { + label: 'x', + borderColor: colors.primary, + backgroundColor: colors.primary, + borderWidth: 2, + data: $imu.map(datapoint => datapoint.x), + yAxisID: 'y' + }, + { + label: 'y', + borderColor: colors.secondary, + backgroundColor: colors.secondary, + borderWidth: 2, + data: $imu.map(datapoint => datapoint.y), + yAxisID: 'y' + }, + { + label: 'z', + borderColor: colors.accent, + backgroundColor: colors.accent, + borderWidth: 2, + data: $imu.map(datapoint => datapoint.z), + yAxisID: 'y' + } + ] + }, + options: { + ...baseConfig, + scales: { + ...baseConfig.scales, + y: { + ...baseConfig.scales.y, + title: { + display: true, + text: 'Angle [°]', + color: colors.background, + font: { size: 16, weight: 'bold' } + } } } } - } - }) + }) + } - tempChart = new Chart(tempChartElement, { - type: 'line', - data: { - datasets: [ - { - label: 'Barometer temperature', - borderColor: colors.secondary, - backgroundColor: colors.secondary, - borderWidth: 2, - data: $imu.map(datapoint => datapoint.bmpTemp), - yAxisID: 'y' - } - ] - }, - options: { - ...baseConfig, - scales: { - ...baseConfig.scales, - y: { - ...baseConfig.scales.y, - title: { - display: true, - text: 'Temperature [C°]', - color: colors.background, - font: { size: 16, weight: 'bold' } + if (tempChartElement) { + tempChart = new Chart(tempChartElement, { + type: 'line', + data: { + datasets: [ + { + label: 'Barometer temperature', + borderColor: colors.secondary, + backgroundColor: colors.secondary, + borderWidth: 2, + data: $imu.map(datapoint => datapoint.bmpTemp), + yAxisID: 'y' + } + ] + }, + options: { + ...baseConfig, + scales: { + ...baseConfig.scales, + y: { + ...baseConfig.scales.y, + title: { + display: true, + text: 'Temperature [C°]', + color: colors.background, + font: { size: 16, weight: 'bold' } + } } } } - } - }) + }) + } - altitudeChart = new Chart(altitudeChartElement, { - type: 'line', - data: { - datasets: [ - { - label: 'Altitude', - borderColor: colors.primary, - backgroundColor: colors.primary, - borderWidth: 2, - data: $imu.map(datapoint => datapoint.altitude), - yAxisID: 'y' - } - ] - }, - options: { - ...baseConfig, - scales: { - ...baseConfig.scales, - y: { - ...baseConfig.scales.y, - title: { - display: true, - text: 'Altitude [M]', - color: colors.background, - font: { size: 16, weight: 'bold' } + if (altitudeChartElement) { + altitudeChart = new Chart(altitudeChartElement, { + type: 'line', + data: { + datasets: [ + { + label: 'Altitude', + borderColor: colors.primary, + backgroundColor: colors.primary, + borderWidth: 2, + data: $imu.map(datapoint => datapoint.altitude), + yAxisID: 'y' + } + ] + }, + options: { + ...baseConfig, + scales: { + ...baseConfig.scales, + y: { + ...baseConfig.scales.y, + title: { + display: true, + text: 'Altitude [M]', + color: colors.background, + font: { size: 16, weight: 'bold' } + } } } } - } - }) + }) + } } const updateChartData = (chart: Chart, data: number[]) => { @@ -185,7 +191,7 @@ } const updateData = () => { - if ($features.imu) { + if ($features.imu && angleChart) { const x = $imu.map(datapoint => datapoint.x) const y = $imu.map(datapoint => datapoint.y) const z = $imu.map(datapoint => datapoint.z) @@ -201,7 +207,7 @@ angleChart.update('none') } - if ($features.bmp) { + if ($features.bmp && tempChart && altitudeChart) { updateChartData( tempChart, $imu.map(datapoint => datapoint.bmpTemp) diff --git a/esp32/include/peripherals/barometer.h b/esp32/include/peripherals/barometer.h index 3898199..75088d8 100644 --- a/esp32/include/peripherals/barometer.h +++ b/esp32/include/peripherals/barometer.h @@ -1,13 +1,8 @@ #pragma once -#include -#include #include - -#include -#include - #include +#include struct BarometerMsg { float pressure {-1}; @@ -20,29 +15,28 @@ class Barometer : public SensorBase { 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; -}; \ No newline at end of file + BMP180Driver _bmp; +}; diff --git a/esp32/include/peripherals/drivers/bmp180.h b/esp32/include/peripherals/drivers/bmp180.h new file mode 100644 index 0000000..8d76dd7 --- /dev/null +++ b/esp32/include/peripherals/drivers/bmp180.h @@ -0,0 +1,129 @@ +#pragma once + +#include +#include + +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; +}; diff --git a/esp32/include/peripherals/drivers/bno055.h b/esp32/include/peripherals/drivers/bno055.h new file mode 100644 index 0000000..7e921d7 --- /dev/null +++ b/esp32/include/peripherals/drivers/bno055.h @@ -0,0 +1,111 @@ +#pragma once + +#include + +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}; +}; diff --git a/esp32/include/peripherals/drivers/hmc5883l.h b/esp32/include/peripherals/drivers/hmc5883l.h new file mode 100644 index 0000000..e641191 --- /dev/null +++ b/esp32/include/peripherals/drivers/hmc5883l.h @@ -0,0 +1,81 @@ +#pragma once + +#include +#include + +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; +}; diff --git a/esp32/include/peripherals/drivers/mpu6050.h b/esp32/include/peripherals/drivers/mpu6050.h new file mode 100644 index 0000000..13cbd4b --- /dev/null +++ b/esp32/include/peripherals/drivers/mpu6050.h @@ -0,0 +1,232 @@ +#pragma once + +#include +#include + +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(x >> 8), static_cast(x & 0xFF), + static_cast(y >> 8), static_cast(y & 0xFF), + static_cast(z >> 8), static_cast(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; +}; diff --git a/esp32/include/peripherals/drivers/pca9685.h b/esp32/include/peripherals/drivers/pca9685.h new file mode 100644 index 0000000..04a7b41 --- /dev/null +++ b/esp32/include/peripherals/drivers/pca9685.h @@ -0,0 +1,119 @@ +#pragma once + +#include +#include + +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(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(on & 0xFF), static_cast(on >> 8), + static_cast(off & 0xFF), static_cast(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; +}; diff --git a/esp32/include/peripherals/gesture.h b/esp32/include/peripherals/gesture.h index 1deeff0..19e27d8 100644 --- a/esp32/include/peripherals/gesture.h +++ b/esp32/include/peripherals/gesture.h @@ -1,6 +1,6 @@ #pragma once -#include +#include 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(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(p), n); } uint8_t readReg(uint8_t reg, void* p, size_t n) { if (!p || !n) return 0; - uint8_t* d = static_cast(p); - wire->beginTransmission(dev); - wire->write(reg); - if (wire->endTransmission(false) != 0) return 0; - uint8_t r = wire->requestFrom(dev, static_cast(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(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; diff --git a/esp32/include/peripherals/i2c_bus.h b/esp32/include/peripherals/i2c_bus.h new file mode 100644 index 0000000..46732b6 --- /dev/null +++ b/esp32/include/peripherals/i2c_bus.h @@ -0,0 +1,160 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +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 scan(uint8_t lower = 1, uint8_t upper = 127) { + std::vector 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; +}; diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h index 8c324c0..580630b 100644 --- a/esp32/include/peripherals/imu.h +++ b/esp32/include/peripherals/imu.h @@ -1,19 +1,17 @@ #pragma once -#include -#include #include +#include +#include #if FT_ENABLED(USE_MPU6050) -#include +#include #endif #if FT_ENABLED(USE_BNO055) -#include +#include #endif -#include - struct IMUAnglesMsg { float rpy[3] {0, 0, 0}; float temperature {-1}; @@ -24,41 +22,20 @@ class IMU : public SensorBase { 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 { 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 { 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 -}; \ No newline at end of file +}; diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h index 93ebdd4..9a830ed 100644 --- a/esp32/include/peripherals/magnetometer.h +++ b/esp32/include/peripherals/magnetometer.h @@ -1,13 +1,8 @@ #pragma once -#include -#include #include - -#include -#include - #include +#include struct MagnetometerMsg { float rpy[3] {0, 0, 0}; @@ -19,34 +14,29 @@ class Magnetometer : public SensorBase { 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; -}; \ No newline at end of file + HMC5883LDriver _mag; +}; diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h index 093306b..490b6f3 100644 --- a/esp32/include/peripherals/peripherals.h +++ b/esp32/include/peripherals/peripherals.h @@ -12,9 +12,9 @@ #include #include -#include #include +#include #include #include #include diff --git a/esp32/include/peripherals/servo_controller.h b/esp32/include/peripherals/servo_controller.h index e7a6f6b..ae8cf7e 100644 --- a/esp32/include/peripherals/servo_controller.h +++ b/esp32/include/peripherals/servo_controller.h @@ -1,16 +1,13 @@ #ifndef ServoController_h #define ServoController_h -#include +#include #include