From 9729d7246e96bea6dda59d88dc4f1beab69ee8b2 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 13:23:45 +0000 Subject: [PATCH 01/12] =?UTF-8?q?=E6=96=B0=E5=BB=BA=20MPU6050?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MPU6050/.keep | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 MPU6050/.keep diff --git a/MPU6050/.keep b/MPU6050/.keep new file mode 100644 index 0000000..e69de29 -- Gitee From d821fdc6452817ef459be2994fbf5c2d872c9ad5 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 13:23:58 +0000 Subject: [PATCH 02/12] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20MP?= =?UTF-8?q?U6050/.keep?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MPU6050/.keep | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 MPU6050/.keep diff --git a/MPU6050/.keep b/MPU6050/.keep deleted file mode 100644 index e69de29..0000000 -- Gitee From c245c229e867ba0a9084173270c371956442740b Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 13:25:39 +0000 Subject: [PATCH 03/12] MPU6050 basic lib Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- MPU6050/mpu6050.c | 255 ++++++++++++++++++++++++++++++++++++++++++++++ MPU6050/mpu6050.h | 65 ++++++++++++ 2 files changed, 320 insertions(+) create mode 100644 MPU6050/mpu6050.c create mode 100644 MPU6050/mpu6050.h diff --git a/MPU6050/mpu6050.c b/MPU6050/mpu6050.c new file mode 100644 index 0000000..5093ad1 --- /dev/null +++ b/MPU6050/mpu6050.c @@ -0,0 +1,255 @@ +#include "mpu6050.h" + +static float gyroX_offset = 0, gyroY_offset = 0, gyroZ_offset = 0; +static float yaw = 0; +static float pitch = 0, roll = 0; + +uint8_t MPU6050_IIC_BUS; +uint8_t accel_cfg = 0, gyro_cfg = 0; + +/** + * @brief 初始化MPU6050 + * @param samplerate_div: 采样率分频器值 (值越小越快 0-255) + * @param gyro_config: 陀螺仪量程 0 - +-250deg/s, 1 - +-500deg/s, 2 - +-1000deg/s, 3 - +-2000deg/s + * @param accel_config: 加速度计量程 0 - +-2g, 1 - +-4g, 2 - +-8g, 3 - +-16g + * @param useint: 是否使能中断 + * @retval None + */ +void MPU6050_Init(uint8_t samplerate_div, uint8_t gyro_config, uint8_t accel_config, uint8_t useint) { + + accel_cfg = accel_config; + gyro_cfg = gyro_config; + + uint8_t data_buf = 0x01; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_PWR_MGMT_1, 1, &data_buf, 1, 0xff); + data_buf = 0x00; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_PWR_MGMT_2, 1, &data_buf, 1, 0xff); + + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_SMPLRT_DIV, 1, &samplerate_div, 1, 0xff); + data_buf = 0x04; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_CONFIG, 1, &data_buf, 1, 0xff); + data_buf = gyro_config << 3; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_GYRO_CONFIG, 1, &data_buf, 1, 0xff); + data_buf = accel_config << 3; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 1, &data_buf, 1, 0xff); + + if (useint) { + data_buf = 0x02; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_INT_PIN_CFG, 1, &data_buf, 1, 0xff); + data_buf = 0x01; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_INT_ENABLE, 1, &data_buf, 1, 0xff); + + MPU6050_ClrInt(); + } +} + +void MPU6050_ClrInt() { + uint8_t dummy; + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_INT_STATUS, 1, &dummy, 1, 100); +} + +/** + * @brief 获取 MPU6050 Accel 和 Gyro 数据寄存器原始数据 + * @param None + * @retval MPU6050_Raw 结构体 + */ +MPU6050_Raw MPU6050_GetRawData() { + MPU6050_Raw data; + uint8_t acc_buf[6], gyro_buf[6]; + + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 1, acc_buf, 6, 100); + + data.AccelX = acc_buf[0] << 8 | acc_buf[1]; + data.AccelY = acc_buf[2] << 8 | acc_buf[3]; + data.AccelZ = acc_buf[4] << 8 | acc_buf[5]; + + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_GYRO_XOUT_H, 1, gyro_buf, 6, 100); + + data.GyroX = gyro_buf[0] << 8 | gyro_buf[1]; + data.GyroY = gyro_buf[2] << 8 | gyro_buf[3]; + data.GyroZ = gyro_buf[4] << 8 | gyro_buf[5]; + + return data; +} + +void MPU6050_RawConv(MPU6050_Raw raw, float* data) { + + float accel_sensitivity; + switch (accel_cfg) { + case 0: accel_sensitivity = 16384.0f; break; // ±2g + case 1: accel_sensitivity = 8192.0f; break; // ±4g + case 2: accel_sensitivity = 4096.0f; break; // ±8g + case 3: accel_sensitivity = 2048.0f; break; // ±16g + default: accel_sensitivity = 16384.0f; break; + } + + float gyro_sensitivity; + switch (gyro_cfg) { + case 0: gyro_sensitivity = 131.0f; break; // ±250dps + case 1: gyro_sensitivity = 65.5f; break; // ±500dps + case 2: gyro_sensitivity = 32.8f; break; // ±1000dps + case 3: gyro_sensitivity = 16.4f; break; // ±2000dps + default: gyro_sensitivity = 131.0f; break; + } + + data[0] = raw.AccelX / accel_sensitivity; + data[1] = raw.AccelY / accel_sensitivity; + data[2] = raw.AccelZ / accel_sensitivity; + + data[3] = (raw.GyroX - gyroX_offset) / gyro_sensitivity; + data[4] = (raw.GyroY - gyroY_offset) / gyro_sensitivity; + data[5] = (raw.GyroZ - gyroZ_offset) / gyro_sensitivity; +} + +/** + * @brief 获取 MPU6050 Temp 数据寄存器原始数据 + * @param None + * @retval MPU6050 Temp 数据寄存器原始数据 + */ +int16_t MPU6050_GetTemp() { + int16_t temp; + uint8_t temp_buf[2]; + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_TEMP_OUT_H, 1, temp_buf, 2, 100); + temp = temp_buf[0] << 8 | temp_buf[1]; + return temp; +} + +/** + * @brief 获取 MPU6050 芯片ID + * @param None + * @retval MPU6050 芯片ID + */ +uint8_t MPU6050_GetID() { + uint8_t id; + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &id, 1, 100); + return id; +} + +/** + * @brief Calibrate Gyroscope + * @param samples Calibration samples + * @retval None + */ +void MPU6050_Calibrate(uint16_t samples) { + MPU6050_Raw data; + + for (int i = 0; i < samples; i++) { + data = MPU6050_GetRawData(); + gyroX_offset += data.GyroX; + gyroY_offset += data.GyroY; + gyroZ_offset += data.GyroZ; + HAL_Delay(1); + } + gyroX_offset /= samples; + gyroY_offset /= samples; + gyroZ_offset /= samples; +} + +/** + * @brief 获取欧拉角 + * @param None + * @retval None + */ +void MPU6050_UpdateEuler() { + + float data_conv[6]; + MPU6050_Raw data = MPU6050_GetRawData(); + + MPU6050_RawConv(data, data_conv); + + float accelX = data_conv[0]; + float accelY = data_conv[1]; + float accelZ = data_conv[2]; + + float gyroX = data_conv[3]; + float gyroY = data_conv[4]; + float gyroZ = data_conv[5]; + + // 计算加速度计角度（弧度） + float pitch_acc = atan2f(accelY, sqrtf(accelX * accelX + accelZ * accelZ)); + float roll_acc = atan2f(accelX, sqrtf(accelY * accelY + accelZ * accelZ)); + + // 转换为角度 + pitch_acc *= 180.0f / 3.1415926536; + roll_acc *= 180.0f / 3.1415926536; + + // 积分得到Yaw角 + yaw += gyroZ * YAW_COMPENSATION_FACTOR; + + // 陀螺仪积分（需初始化时用加速度计角度赋值） + static uint8_t is_first_run = 1; + if (is_first_run) { + pitch = pitch_acc; + roll = roll_acc; + is_first_run = 0; + } else { + pitch += gyroY * dt; // 陀螺仪Y轴对应俯仰角速度 + roll += gyroX * dt; // 陀螺仪X轴对应横滚角速度 + } + + // 短期修正：当设备静止时，若加速度计稳定，可重置漂移（需自定义条件） + float accelMagnitude = sqrtf( + data.AccelX * data.AccelX + + data.AccelY * data.AccelY + + data.AccelZ * data.AccelZ + ); + if (fabs(accelMagnitude - 1.0f) < 0.1f) { // 加速度接近1g时认为静止 + // 可在此处重置Yaw或补偿漂移（例如：yaw *= 0.99;） + } + + // 互补滤波（α=0.96，权重陀螺仪） + float alpha = 0.96f; + pitch = alpha * pitch + (1 - alpha) * pitch_acc; + roll = alpha * roll + (1 - alpha) * roll_acc; +} + + +/** + * @brief 获取Yaw + * @param None + * @retval None + */ +void MPU6050_UpdateYaw() { + + float gyro_sensitivity; + switch (gyro_cfg) { + case 0: gyro_sensitivity = 131.0f; break; // ±250dps + case 1: gyro_sensitivity = 65.5f; break; // ±500dps + case 2: gyro_sensitivity = 32.8f; break; // ±1000dps + case 3: gyro_sensitivity = 16.4f; break; // ±2000dps + default: gyro_sensitivity = 131.0f; break; + } + + MPU6050_Raw data = MPU6050_GetRawData(); + float gyroZ = (data.GyroZ - gyroZ_offset) / gyro_sensitivity; + + yaw += gyroZ * YAW_COMPENSATION_FACTOR; +} + + +/** + * @brief 获取俯仰角 + * @param None + * @retval pitch + */ +float MPU6050_GetPitch() { + return pitch; +} + +/** + * @brief 获取横滚角 + * @param None + * @retval roll + */ +float MPU6050_GetRoll() { + return roll; +} + +/** + * @brief 获取偏航角 + * @param None + * @retval yaw + */ +float MPU6050_GetYaw() { + return yaw; +} diff --git a/MPU6050/mpu6050.h b/MPU6050/mpu6050.h new file mode 100644 index 0000000..7570218 --- /dev/null +++ b/MPU6050/mpu6050.h @@ -0,0 +1,65 @@ +#ifndef __MPU6050_H__ +#define __MPU6050_H__ + +#include "i2c.h" +#include + +#define MPU6050_ADDR 0xD0 + +#define MPU6050_SMPLRT_DIV 0x19 +#define MPU6050_CONFIG 0x1A +#define MPU6050_GYRO_CONFIG 0x1B +#define MPU6050_ACCEL_CONFIG 0x1C + +#define MPU6050_ACCEL_XOUT_H 0x3B +#define MPU6050_ACCEL_XOUT_L 0x3C +#define MPU6050_ACCEL_YOUT_H 0x3D +#define MPU6050_ACCEL_YOUT_L 0x3E +#define MPU6050_ACCEL_ZOUT_H 0x3F +#define MPU6050_ACCEL_ZOUT_L 0x40 +#define MPU6050_TEMP_OUT_H 0x41 +#define MPU6050_TEMP_OUT_L 0x42 +#define MPU6050_GYRO_XOUT_H 0x43 +#define MPU6050_GYRO_XOUT_L 0x44 +#define MPU6050_GYRO_YOUT_H 0x45 +#define MPU6050_GYRO_YOUT_L 0x46 +#define MPU6050_GYRO_ZOUT_H 0x47 +#define MPU6050_GYRO_ZOUT_L 0x48 + +#define MPU6050_PWR_MGMT_1 0x6B +#define MPU6050_PWR_MGMT_2 0x6C +#define MPU6050_WHO_AM_I 0x75 + +#define MPU6050_INT_ENABLE 0x38 +#define MPU6050_INT_PIN_CFG 0x37 +#define MPU6050_INT_STATUS 0x3A + +#define dt 0.001f +#define YAW_COMPENSATION_FACTOR 0.0097262f + +typedef struct { + int16_t AccelX; + int16_t AccelY; + int16_t AccelZ; + int16_t GyroX; + int16_t GyroY; + int16_t GyroZ; +} MPU6050_Raw; + +void MPU6050_Init(uint8_t samplerate_div, uint8_t gyro_config, uint8_t accel_config, uint8_t useint); +void MPU6050_ClrInt(); + +MPU6050_Raw MPU6050_GetRawData(); +void MPU6050_RawConv(MPU6050_Raw raw, float* data); +int16_t MPU6050_GetTemp(); +uint8_t MPU6050_GetID(); + +void MPU6050_Calibrate(uint16_t samples); +void MPU6050_UpdateEuler(); +void MPU6050_UpdateYaw(); + +float MPU6050_GetPitch(); +float MPU6050_GetRoll(); +float MPU6050_GetYaw(); + +#endif -- Gitee From ce79b0c68312a5b0f55cae37a160f27ea8cde1d0 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 13:35:25 +0000 Subject: [PATCH 04/12] add MPU6050. Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- MPU6050/readme.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 MPU6050/readme.md diff --git a/MPU6050/readme.md b/MPU6050/readme.md new file mode 100644 index 0000000..e69de29 -- Gitee From e8a02681daa4602d6fbdfb6b45bf931307454854 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 13:50:35 +0000 Subject: [PATCH 05/12] update MPU6050/readme.md. Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- MPU6050/readme.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/MPU6050/readme.md b/MPU6050/readme.md index e69de29..84d55db 100644 --- a/MPU6050/readme.md +++ b/MPU6050/readme.md @@ -0,0 +1,10 @@ +## 此库为基本MPU6050库，能实现基本的获取数据，计算/获取欧拉角的功能。 +## 基于HAL，默认使用hi2c2。若需使用其他IIC接口请自行修改所有`&hi2c2`为需要的值 + +### *注意事项：* +- 若需获取欧拉角，请先调用MPU6050_Calibrate()函数，校准零飘。 +- **此库为自己乱写所以功能有欠缺：若需获取偏航角YAW，还需调整头文件中`YAW_COMPENSATION_FACTOR`一值，使其对应实际角度值** + + **例如：当前YAW值为0.0，转动传感器180.0°后，若输出的值为205.0，则需修改`YAW_COMPENSATION_FACTOR`为原来的180.0/205.0倍。** + + **也即：若输出的值为x，则需修改`YAW_COMPENSATION_FACTOR`为原来的180.0/x倍。** \ No newline at end of file -- Gitee From b5185b875adc6105ebdff0741f604f5e9e91e1ae Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:32:16 +0000 Subject: [PATCH 06/12] TCS34725 basic lib Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- MPU6050/TCS34725/Inc/tcs34725.h | 22 +++++++++++++ MPU6050/TCS34725/Src/tcs34725.c | 56 +++++++++++++++++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 MPU6050/TCS34725/Inc/tcs34725.h create mode 100644 MPU6050/TCS34725/Src/tcs34725.c diff --git a/MPU6050/TCS34725/Inc/tcs34725.h b/MPU6050/TCS34725/Inc/tcs34725.h new file mode 100644 index 0000000..8c83242 --- /dev/null +++ b/MPU6050/TCS34725/Inc/tcs34725.h @@ -0,0 +1,22 @@ +#include "i2c.h" + +typedef struct { + uint16_t clear; + uint16_t red; + uint16_t green; + uint16_t blue; +} TCS34725_RawData; + +#define COMMAND_BIT 0x80 + +#define TCS34725_ADDR (0x29 << 1) +#define TCS34725_ENABLE_REG (COMMAND_BIT | 0x00) +#define TCS34725_ATIME_REG (COMMAND_BIT | 0x01) +#define TCS34725_CONTROL_REG (COMMAND_BIT | 0x0F) +#define TCS34725_ID_REG (COMMAND_BIT | 0x12) +#define TCS34725_STATUS_REG (COMMAND_BIT | 0x13) +#define TCS34725_CDATA_REG (COMMAND_BIT | 0x14) + +uint8_t TCS34725_Init(I2C_HandleTypeDef *hi2c, uint8_t atime, uint8_t gain); +TCS34725_RawData TCS34725_GetRawData(I2C_HandleTypeDef *hi2c); +uint8_t TCS34725_DataReady(I2C_HandleTypeDef *hi2c); diff --git a/MPU6050/TCS34725/Src/tcs34725.c b/MPU6050/TCS34725/Src/tcs34725.c new file mode 100644 index 0000000..44324ab --- /dev/null +++ b/MPU6050/TCS34725/Src/tcs34725.c @@ -0,0 +1,56 @@ +#include "tcs34725.h" + +/** + * @brief 初始化TCS34725传感器 + * @param hi2c: I2C句柄 + * @param atime: 设置积分时间 (0x00 - 0xFF, 越大时间越短, 但精度越低) int.time = (256 - atime) × 2.4ms + * @param gain: 设置增益 (0x00 - 1x, 0x01 - 4x, 0x02 - 16x, 0x03 - 60x) + * @retval 1: 初始化成功, 0: 初始化失败 + */ +uint8_t TCS34725_Init(I2C_HandleTypeDef *hi2c, uint8_t atime, uint8_t gain) { + uint8_t device_id; + HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_ID_REG, 1, &device_id, 1, 100); + + if (device_id != 0x44 && device_id != 0x4D) { return 0; } + + HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_ATIME_REG, 1, &atime, 1, 100); + + HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_CONTROL_REG, 1, &gain, 1, 100); + + uint8_t enable = 0x03; // PON = 1, AEN = 1 + HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_ENABLE_REG, 1, &enable, 1, 100); + + HAL_Delay(50); + return 1; +} + +/** + * @brief 获取TCS34725的 RGBC + * @param data: 存储 RGBC 数据 + * @retval None + */ +TCS34725_RawData TCS34725_GetRawData(I2C_HandleTypeDef *hi2c) { + uint8_t raw_data[8]; + TCS34725_RawData data; + + HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_CDATA_REG, 1, raw_data, 8, 100); + + // 组合高低字节，数据低字节在前 + data.clear = (raw_data[1] << 8) | raw_data[0]; + data.red = (raw_data[3] << 8) | raw_data[2]; + data.green = (raw_data[5] << 8) | raw_data[4]; + data.blue = (raw_data[7] << 8) | raw_data[6]; + + return data; +} + +/** + * @brief 检查TCS34725数据是否准备好 + * @param hi2c: I2C句柄 + * @retval 1: 数据准备好, 0: 数据未准备好 + */ +uint8_t TCS34725_DataReady(I2C_HandleTypeDef *hi2c) { + uint8_t status; + HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_STATUS_REG, 1, &status, 1, 100); + return (status & 0x01); +} -- Gitee From 87e4573a62cb3a8bb057692bd7dc3757a33b1d70 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:32:40 +0000 Subject: [PATCH 07/12] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20MP?= =?UTF-8?q?U6050/TCS34725?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MPU6050/TCS34725/Inc/tcs34725.h | 22 ------------- MPU6050/TCS34725/Src/tcs34725.c | 56 --------------------------------- 2 files changed, 78 deletions(-) delete mode 100644 MPU6050/TCS34725/Inc/tcs34725.h delete mode 100644 MPU6050/TCS34725/Src/tcs34725.c diff --git a/MPU6050/TCS34725/Inc/tcs34725.h b/MPU6050/TCS34725/Inc/tcs34725.h deleted file mode 100644 index 8c83242..0000000 --- a/MPU6050/TCS34725/Inc/tcs34725.h +++ /dev/null @@ -1,22 +0,0 @@ -#include "i2c.h" - -typedef struct { - uint16_t clear; - uint16_t red; - uint16_t green; - uint16_t blue; -} TCS34725_RawData; - -#define COMMAND_BIT 0x80 - -#define TCS34725_ADDR (0x29 << 1) -#define TCS34725_ENABLE_REG (COMMAND_BIT | 0x00) -#define TCS34725_ATIME_REG (COMMAND_BIT | 0x01) -#define TCS34725_CONTROL_REG (COMMAND_BIT | 0x0F) -#define TCS34725_ID_REG (COMMAND_BIT | 0x12) -#define TCS34725_STATUS_REG (COMMAND_BIT | 0x13) -#define TCS34725_CDATA_REG (COMMAND_BIT | 0x14) - -uint8_t TCS34725_Init(I2C_HandleTypeDef *hi2c, uint8_t atime, uint8_t gain); -TCS34725_RawData TCS34725_GetRawData(I2C_HandleTypeDef *hi2c); -uint8_t TCS34725_DataReady(I2C_HandleTypeDef *hi2c); diff --git a/MPU6050/TCS34725/Src/tcs34725.c b/MPU6050/TCS34725/Src/tcs34725.c deleted file mode 100644 index 44324ab..0000000 --- a/MPU6050/TCS34725/Src/tcs34725.c +++ /dev/null @@ -1,56 +0,0 @@ -#include "tcs34725.h" - -/** - * @brief 初始化TCS34725传感器 - * @param hi2c: I2C句柄 - * @param atime: 设置积分时间 (0x00 - 0xFF, 越大时间越短, 但精度越低) int.time = (256 - atime) × 2.4ms - * @param gain: 设置增益 (0x00 - 1x, 0x01 - 4x, 0x02 - 16x, 0x03 - 60x) - * @retval 1: 初始化成功, 0: 初始化失败 - */ -uint8_t TCS34725_Init(I2C_HandleTypeDef *hi2c, uint8_t atime, uint8_t gain) { - uint8_t device_id; - HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_ID_REG, 1, &device_id, 1, 100); - - if (device_id != 0x44 && device_id != 0x4D) { return 0; } - - HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_ATIME_REG, 1, &atime, 1, 100); - - HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_CONTROL_REG, 1, &gain, 1, 100); - - uint8_t enable = 0x03; // PON = 1, AEN = 1 - HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_ENABLE_REG, 1, &enable, 1, 100); - - HAL_Delay(50); - return 1; -} - -/** - * @brief 获取TCS34725的 RGBC - * @param data: 存储 RGBC 数据 - * @retval None - */ -TCS34725_RawData TCS34725_GetRawData(I2C_HandleTypeDef *hi2c) { - uint8_t raw_data[8]; - TCS34725_RawData data; - - HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_CDATA_REG, 1, raw_data, 8, 100); - - // 组合高低字节，数据低字节在前 - data.clear = (raw_data[1] << 8) | raw_data[0]; - data.red = (raw_data[3] << 8) | raw_data[2]; - data.green = (raw_data[5] << 8) | raw_data[4]; - data.blue = (raw_data[7] << 8) | raw_data[6]; - - return data; -} - -/** - * @brief 检查TCS34725数据是否准备好 - * @param hi2c: I2C句柄 - * @retval 1: 数据准备好, 0: 数据未准备好 - */ -uint8_t TCS34725_DataReady(I2C_HandleTypeDef *hi2c) { - uint8_t status; - HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_STATUS_REG, 1, &status, 1, 100); - return (status & 0x01); -} -- Gitee From 60f7306379f0d870f27da57a13593c3f226469bb Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:33:06 +0000 Subject: [PATCH 08/12] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20MP?= =?UTF-8?q?U6050/mpu6050.c?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MPU6050/mpu6050.c | 255 ---------------------------------------------- 1 file changed, 255 deletions(-) delete mode 100644 MPU6050/mpu6050.c diff --git a/MPU6050/mpu6050.c b/MPU6050/mpu6050.c deleted file mode 100644 index 5093ad1..0000000 --- a/MPU6050/mpu6050.c +++ /dev/null @@ -1,255 +0,0 @@ -#include "mpu6050.h" - -static float gyroX_offset = 0, gyroY_offset = 0, gyroZ_offset = 0; -static float yaw = 0; -static float pitch = 0, roll = 0; - -uint8_t MPU6050_IIC_BUS; -uint8_t accel_cfg = 0, gyro_cfg = 0; - -/** - * @brief 初始化MPU6050 - * @param samplerate_div: 采样率分频器值 (值越小越快 0-255) - * @param gyro_config: 陀螺仪量程 0 - +-250deg/s, 1 - +-500deg/s, 2 - +-1000deg/s, 3 - +-2000deg/s - * @param accel_config: 加速度计量程 0 - +-2g, 1 - +-4g, 2 - +-8g, 3 - +-16g - * @param useint: 是否使能中断 - * @retval None - */ -void MPU6050_Init(uint8_t samplerate_div, uint8_t gyro_config, uint8_t accel_config, uint8_t useint) { - - accel_cfg = accel_config; - gyro_cfg = gyro_config; - - uint8_t data_buf = 0x01; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_PWR_MGMT_1, 1, &data_buf, 1, 0xff); - data_buf = 0x00; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_PWR_MGMT_2, 1, &data_buf, 1, 0xff); - - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_SMPLRT_DIV, 1, &samplerate_div, 1, 0xff); - data_buf = 0x04; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_CONFIG, 1, &data_buf, 1, 0xff); - data_buf = gyro_config << 3; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_GYRO_CONFIG, 1, &data_buf, 1, 0xff); - data_buf = accel_config << 3; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 1, &data_buf, 1, 0xff); - - if (useint) { - data_buf = 0x02; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_INT_PIN_CFG, 1, &data_buf, 1, 0xff); - data_buf = 0x01; - HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_INT_ENABLE, 1, &data_buf, 1, 0xff); - - MPU6050_ClrInt(); - } -} - -void MPU6050_ClrInt() { - uint8_t dummy; - HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_INT_STATUS, 1, &dummy, 1, 100); -} - -/** - * @brief 获取 MPU6050 Accel 和 Gyro 数据寄存器原始数据 - * @param None - * @retval MPU6050_Raw 结构体 - */ -MPU6050_Raw MPU6050_GetRawData() { - MPU6050_Raw data; - uint8_t acc_buf[6], gyro_buf[6]; - - HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 1, acc_buf, 6, 100); - - data.AccelX = acc_buf[0] << 8 | acc_buf[1]; - data.AccelY = acc_buf[2] << 8 | acc_buf[3]; - data.AccelZ = acc_buf[4] << 8 | acc_buf[5]; - - HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_GYRO_XOUT_H, 1, gyro_buf, 6, 100); - - data.GyroX = gyro_buf[0] << 8 | gyro_buf[1]; - data.GyroY = gyro_buf[2] << 8 | gyro_buf[3]; - data.GyroZ = gyro_buf[4] << 8 | gyro_buf[5]; - - return data; -} - -void MPU6050_RawConv(MPU6050_Raw raw, float* data) { - - float accel_sensitivity; - switch (accel_cfg) { - case 0: accel_sensitivity = 16384.0f; break; // ±2g - case 1: accel_sensitivity = 8192.0f; break; // ±4g - case 2: accel_sensitivity = 4096.0f; break; // ±8g - case 3: accel_sensitivity = 2048.0f; break; // ±16g - default: accel_sensitivity = 16384.0f; break; - } - - float gyro_sensitivity; - switch (gyro_cfg) { - case 0: gyro_sensitivity = 131.0f; break; // ±250dps - case 1: gyro_sensitivity = 65.5f; break; // ±500dps - case 2: gyro_sensitivity = 32.8f; break; // ±1000dps - case 3: gyro_sensitivity = 16.4f; break; // ±2000dps - default: gyro_sensitivity = 131.0f; break; - } - - data[0] = raw.AccelX / accel_sensitivity; - data[1] = raw.AccelY / accel_sensitivity; - data[2] = raw.AccelZ / accel_sensitivity; - - data[3] = (raw.GyroX - gyroX_offset) / gyro_sensitivity; - data[4] = (raw.GyroY - gyroY_offset) / gyro_sensitivity; - data[5] = (raw.GyroZ - gyroZ_offset) / gyro_sensitivity; -} - -/** - * @brief 获取 MPU6050 Temp 数据寄存器原始数据 - * @param None - * @retval MPU6050 Temp 数据寄存器原始数据 - */ -int16_t MPU6050_GetTemp() { - int16_t temp; - uint8_t temp_buf[2]; - HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_TEMP_OUT_H, 1, temp_buf, 2, 100); - temp = temp_buf[0] << 8 | temp_buf[1]; - return temp; -} - -/** - * @brief 获取 MPU6050 芯片ID - * @param None - * @retval MPU6050 芯片ID - */ -uint8_t MPU6050_GetID() { - uint8_t id; - HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &id, 1, 100); - return id; -} - -/** - * @brief Calibrate Gyroscope - * @param samples Calibration samples - * @retval None - */ -void MPU6050_Calibrate(uint16_t samples) { - MPU6050_Raw data; - - for (int i = 0; i < samples; i++) { - data = MPU6050_GetRawData(); - gyroX_offset += data.GyroX; - gyroY_offset += data.GyroY; - gyroZ_offset += data.GyroZ; - HAL_Delay(1); - } - gyroX_offset /= samples; - gyroY_offset /= samples; - gyroZ_offset /= samples; -} - -/** - * @brief 获取欧拉角 - * @param None - * @retval None - */ -void MPU6050_UpdateEuler() { - - float data_conv[6]; - MPU6050_Raw data = MPU6050_GetRawData(); - - MPU6050_RawConv(data, data_conv); - - float accelX = data_conv[0]; - float accelY = data_conv[1]; - float accelZ = data_conv[2]; - - float gyroX = data_conv[3]; - float gyroY = data_conv[4]; - float gyroZ = data_conv[5]; - - // 计算加速度计角度（弧度） - float pitch_acc = atan2f(accelY, sqrtf(accelX * accelX + accelZ * accelZ)); - float roll_acc = atan2f(accelX, sqrtf(accelY * accelY + accelZ * accelZ)); - - // 转换为角度 - pitch_acc *= 180.0f / 3.1415926536; - roll_acc *= 180.0f / 3.1415926536; - - // 积分得到Yaw角 - yaw += gyroZ * YAW_COMPENSATION_FACTOR; - - // 陀螺仪积分（需初始化时用加速度计角度赋值） - static uint8_t is_first_run = 1; - if (is_first_run) { - pitch = pitch_acc; - roll = roll_acc; - is_first_run = 0; - } else { - pitch += gyroY * dt; // 陀螺仪Y轴对应俯仰角速度 - roll += gyroX * dt; // 陀螺仪X轴对应横滚角速度 - } - - // 短期修正：当设备静止时，若加速度计稳定，可重置漂移（需自定义条件） - float accelMagnitude = sqrtf( - data.AccelX * data.AccelX + - data.AccelY * data.AccelY + - data.AccelZ * data.AccelZ - ); - if (fabs(accelMagnitude - 1.0f) < 0.1f) { // 加速度接近1g时认为静止 - // 可在此处重置Yaw或补偿漂移（例如：yaw *= 0.99;） - } - - // 互补滤波（α=0.96，权重陀螺仪） - float alpha = 0.96f; - pitch = alpha * pitch + (1 - alpha) * pitch_acc; - roll = alpha * roll + (1 - alpha) * roll_acc; -} - - -/** - * @brief 获取Yaw - * @param None - * @retval None - */ -void MPU6050_UpdateYaw() { - - float gyro_sensitivity; - switch (gyro_cfg) { - case 0: gyro_sensitivity = 131.0f; break; // ±250dps - case 1: gyro_sensitivity = 65.5f; break; // ±500dps - case 2: gyro_sensitivity = 32.8f; break; // ±1000dps - case 3: gyro_sensitivity = 16.4f; break; // ±2000dps - default: gyro_sensitivity = 131.0f; break; - } - - MPU6050_Raw data = MPU6050_GetRawData(); - float gyroZ = (data.GyroZ - gyroZ_offset) / gyro_sensitivity; - - yaw += gyroZ * YAW_COMPENSATION_FACTOR; -} - - -/** - * @brief 获取俯仰角 - * @param None - * @retval pitch - */ -float MPU6050_GetPitch() { - return pitch; -} - -/** - * @brief 获取横滚角 - * @param None - * @retval roll - */ -float MPU6050_GetRoll() { - return roll; -} - -/** - * @brief 获取偏航角 - * @param None - * @retval yaw - */ -float MPU6050_GetYaw() { - return yaw; -} -- Gitee From d0c7089cb6fa1e7a9041e6ee428a6060f7dfa1e3 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:33:13 +0000 Subject: [PATCH 09/12] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=96=87=E4=BB=B6=20MP?= =?UTF-8?q?U6050/mpu6050.h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MPU6050/mpu6050.h | 65 ----------------------------------------------- 1 file changed, 65 deletions(-) delete mode 100644 MPU6050/mpu6050.h diff --git a/MPU6050/mpu6050.h b/MPU6050/mpu6050.h deleted file mode 100644 index 7570218..0000000 --- a/MPU6050/mpu6050.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef __MPU6050_H__ -#define __MPU6050_H__ - -#include "i2c.h" -#include - -#define MPU6050_ADDR 0xD0 - -#define MPU6050_SMPLRT_DIV 0x19 -#define MPU6050_CONFIG 0x1A -#define MPU6050_GYRO_CONFIG 0x1B -#define MPU6050_ACCEL_CONFIG 0x1C - -#define MPU6050_ACCEL_XOUT_H 0x3B -#define MPU6050_ACCEL_XOUT_L 0x3C -#define MPU6050_ACCEL_YOUT_H 0x3D -#define MPU6050_ACCEL_YOUT_L 0x3E -#define MPU6050_ACCEL_ZOUT_H 0x3F -#define MPU6050_ACCEL_ZOUT_L 0x40 -#define MPU6050_TEMP_OUT_H 0x41 -#define MPU6050_TEMP_OUT_L 0x42 -#define MPU6050_GYRO_XOUT_H 0x43 -#define MPU6050_GYRO_XOUT_L 0x44 -#define MPU6050_GYRO_YOUT_H 0x45 -#define MPU6050_GYRO_YOUT_L 0x46 -#define MPU6050_GYRO_ZOUT_H 0x47 -#define MPU6050_GYRO_ZOUT_L 0x48 - -#define MPU6050_PWR_MGMT_1 0x6B -#define MPU6050_PWR_MGMT_2 0x6C -#define MPU6050_WHO_AM_I 0x75 - -#define MPU6050_INT_ENABLE 0x38 -#define MPU6050_INT_PIN_CFG 0x37 -#define MPU6050_INT_STATUS 0x3A - -#define dt 0.001f -#define YAW_COMPENSATION_FACTOR 0.0097262f - -typedef struct { - int16_t AccelX; - int16_t AccelY; - int16_t AccelZ; - int16_t GyroX; - int16_t GyroY; - int16_t GyroZ; -} MPU6050_Raw; - -void MPU6050_Init(uint8_t samplerate_div, uint8_t gyro_config, uint8_t accel_config, uint8_t useint); -void MPU6050_ClrInt(); - -MPU6050_Raw MPU6050_GetRawData(); -void MPU6050_RawConv(MPU6050_Raw raw, float* data); -int16_t MPU6050_GetTemp(); -uint8_t MPU6050_GetID(); - -void MPU6050_Calibrate(uint16_t samples); -void MPU6050_UpdateEuler(); -void MPU6050_UpdateYaw(); - -float MPU6050_GetPitch(); -float MPU6050_GetRoll(); -float MPU6050_GetYaw(); - -#endif -- Gitee From eddef4e6a452fcb05232fd8f94f35a67f07588b4 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:34:21 +0000 Subject: [PATCH 10/12] MPU6050 basic lib Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- MPU6050/Inc/mpu6050.h | 67 +++++++++++ MPU6050/Src/mpu6050.c | 266 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 333 insertions(+) create mode 100644 MPU6050/Inc/mpu6050.h create mode 100644 MPU6050/Src/mpu6050.c diff --git a/MPU6050/Inc/mpu6050.h b/MPU6050/Inc/mpu6050.h new file mode 100644 index 0000000..44bbfc4 --- /dev/null +++ b/MPU6050/Inc/mpu6050.h @@ -0,0 +1,67 @@ +#ifndef __MPU6050_H__ +#define __MPU6050_H__ + +#include "i2c.h" +#include + +#define MPU6050_ADDR 0xD0 + +#define MPU6050_SMPLRT_DIV 0x19 +#define MPU6050_CONFIG 0x1A +#define MPU6050_GYRO_CONFIG 0x1B +#define MPU6050_ACCEL_CONFIG 0x1C + +#define MPU6050_ACCEL_XOUT_H 0x3B +#define MPU6050_ACCEL_XOUT_L 0x3C +#define MPU6050_ACCEL_YOUT_H 0x3D +#define MPU6050_ACCEL_YOUT_L 0x3E +#define MPU6050_ACCEL_ZOUT_H 0x3F +#define MPU6050_ACCEL_ZOUT_L 0x40 +#define MPU6050_TEMP_OUT_H 0x41 +#define MPU6050_TEMP_OUT_L 0x42 +#define MPU6050_GYRO_XOUT_H 0x43 +#define MPU6050_GYRO_XOUT_L 0x44 +#define MPU6050_GYRO_YOUT_H 0x45 +#define MPU6050_GYRO_YOUT_L 0x46 +#define MPU6050_GYRO_ZOUT_H 0x47 +#define MPU6050_GYRO_ZOUT_L 0x48 + +#define MPU6050_PWR_MGMT_1 0x6B +#define MPU6050_PWR_MGMT_2 0x6C +#define MPU6050_WHO_AM_I 0x75 + +#define MPU6050_INT_ENABLE 0x38 +#define MPU6050_INT_PIN_CFG 0x37 +#define MPU6050_INT_STATUS 0x3A + +#define dt 0.001f + +// 根据实际情况修改此值 +#define YAW_COMPENSATION_FACTOR 0.0097262f + +typedef struct { + int16_t AccelX; + int16_t AccelY; + int16_t AccelZ; + int16_t GyroX; + int16_t GyroY; + int16_t GyroZ; +} MPU6050_Raw; + +void MPU6050_Init(uint8_t samplerate_div, uint8_t gyro_config, uint8_t accel_config, uint8_t useint); +void MPU6050_ClrInt(); + +MPU6050_Raw MPU6050_GetRawData(); +void MPU6050_RawConv(MPU6050_Raw raw, float* data); +int16_t MPU6050_GetTemp(); +uint8_t MPU6050_GetID(); + +void MPU6050_Calibrate(uint16_t samples); +void MPU6050_UpdateEuler(); +void MPU6050_UpdateYaw(); + +float MPU6050_GetPitch(); +float MPU6050_GetRoll(); +float MPU6050_GetYaw(); + +#endif diff --git a/MPU6050/Src/mpu6050.c b/MPU6050/Src/mpu6050.c new file mode 100644 index 0000000..8d87ad5 --- /dev/null +++ b/MPU6050/Src/mpu6050.c @@ -0,0 +1,266 @@ +#include "mpu6050.h" + +static float gyroX_offset = 0, gyroY_offset = 0, gyroZ_offset = 0; +static float yaw = 0; +static float pitch = 0, roll = 0; + +uint8_t MPU6050_IIC_BUS; +uint8_t accel_cfg = 0, gyro_cfg = 0; + +/** + * @brief 初始化MPU6050 + * @param samplerate_div: 采样率分频器值 (值越小越快 0-255) + * @param gyro_config: 陀螺仪量程 0 - +-250deg/s, 1 - +-500deg/s, 2 - +-1000deg/s, 3 - +-2000deg/s + * @param accel_config: 加速度计量程 0 - +-2g, 1 - +-4g, 2 - +-8g, 3 - +-16g + * @param useint: 是否使能中断 + * @retval None + */ +void MPU6050_Init(uint8_t samplerate_div, uint8_t gyro_config, uint8_t accel_config, uint8_t useint) { + + accel_cfg = accel_config; + gyro_cfg = gyro_config; + + uint8_t data_buf = 0x01; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_PWR_MGMT_1, 1, &data_buf, 1, 0xff); + data_buf = 0x00; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_PWR_MGMT_2, 1, &data_buf, 1, 0xff); + + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_SMPLRT_DIV, 1, &samplerate_div, 1, 0xff); + data_buf = 0x04; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_CONFIG, 1, &data_buf, 1, 0xff); + data_buf = gyro_config << 3; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_GYRO_CONFIG, 1, &data_buf, 1, 0xff); + data_buf = accel_config << 3; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 1, &data_buf, 1, 0xff); + + if (useint) { + data_buf = 0x02; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_INT_PIN_CFG, 1, &data_buf, 1, 0xff); + data_buf = 0x01; + HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDR, MPU6050_INT_ENABLE, 1, &data_buf, 1, 0xff); + + MPU6050_ClrInt(); + } +} + +/** + * @brief 清除MPU6050中断 + * @param None + * @retval None + */ +void MPU6050_ClrInt() { + uint8_t dummy; + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_INT_STATUS, 1, &dummy, 1, 100); +} + +/** + * @brief 获取 MPU6050 Accel 和 Gyro 数据寄存器原始数据 + * @param None + * @retval MPU6050_Raw 结构体 + */ +MPU6050_Raw MPU6050_GetRawData() { + MPU6050_Raw data; + uint8_t acc_buf[6], gyro_buf[6]; + + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 1, acc_buf, 6, 100); + + data.AccelX = acc_buf[0] << 8 | acc_buf[1]; + data.AccelY = acc_buf[2] << 8 | acc_buf[3]; + data.AccelZ = acc_buf[4] << 8 | acc_buf[5]; + + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_GYRO_XOUT_H, 1, gyro_buf, 6, 100); + + data.GyroX = gyro_buf[0] << 8 | gyro_buf[1]; + data.GyroY = gyro_buf[2] << 8 | gyro_buf[3]; + data.GyroZ = gyro_buf[4] << 8 | gyro_buf[5]; + + return data; +} + +/** + * @brief 转换原始数据到 SI 单位 + * @param raw 原始数据 + * @param data 转换后的数据 + * @retval None + */ +void MPU6050_RawConv(MPU6050_Raw raw, float* data) { + + float accel_sensitivity; + switch (accel_cfg) { + case 0: accel_sensitivity = 16384.0f; break; // ±2g + case 1: accel_sensitivity = 8192.0f; break; // ±4g + case 2: accel_sensitivity = 4096.0f; break; // ±8g + case 3: accel_sensitivity = 2048.0f; break; // ±16g + default: accel_sensitivity = 16384.0f; break; + } + + float gyro_sensitivity; + switch (gyro_cfg) { + case 0: gyro_sensitivity = 131.0f; break; // ±250dps + case 1: gyro_sensitivity = 65.5f; break; // ±500dps + case 2: gyro_sensitivity = 32.8f; break; // ±1000dps + case 3: gyro_sensitivity = 16.4f; break; // ±2000dps + default: gyro_sensitivity = 131.0f; break; + } + + data[0] = raw.AccelX / accel_sensitivity; + data[1] = raw.AccelY / accel_sensitivity; + data[2] = raw.AccelZ / accel_sensitivity; + + data[3] = (raw.GyroX - gyroX_offset) / gyro_sensitivity; + data[4] = (raw.GyroY - gyroY_offset) / gyro_sensitivity; + data[5] = (raw.GyroZ - gyroZ_offset) / gyro_sensitivity; +} + +/** + * @brief 获取 MPU6050 Temp 数据寄存器原始数据 + * @param None + * @retval MPU6050 Temp 数据寄存器原始数据 + */ +int16_t MPU6050_GetTemp() { + int16_t temp; + uint8_t temp_buf[2]; + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_TEMP_OUT_H, 1, temp_buf, 2, 100); + temp = temp_buf[0] << 8 | temp_buf[1]; + return temp; +} + +/** + * @brief 获取 MPU6050 芯片ID + * @param None + * @retval MPU6050 芯片ID + */ +uint8_t MPU6050_GetID() { + uint8_t id; + HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &id, 1, 100); + return id; +} + +/** + * @brief 校准陀螺仪 + * @param samples 校准样本数 + * @retval None + */ +void MPU6050_Calibrate(uint16_t samples) { + MPU6050_Raw data; + + for (int i = 0; i < samples; i++) { + data = MPU6050_GetRawData(); + gyroX_offset += data.GyroX; + gyroY_offset += data.GyroY; + gyroZ_offset += data.GyroZ; + HAL_Delay(1); + } + gyroX_offset /= samples; + gyroY_offset /= samples; + gyroZ_offset /= samples; +} + +/** + * @brief 计算并更新欧拉角（不是获取） + * @param None + * @retval None + */ +void MPU6050_UpdateEuler() { + + float data_conv[6]; + MPU6050_Raw data = MPU6050_GetRawData(); + + MPU6050_RawConv(data, data_conv); + + float accelX = data_conv[0]; + float accelY = data_conv[1]; + float accelZ = data_conv[2]; + + float gyroX = data_conv[3]; + float gyroY = data_conv[4]; + float gyroZ = data_conv[5]; + + // 计算加速度计角度（弧度） + float pitch_acc = atan2f(accelY, sqrtf(accelX * accelX + accelZ * accelZ)); + float roll_acc = atan2f(accelX, sqrtf(accelY * accelY + accelZ * accelZ)); + + // 转换为角度 + pitch_acc *= 180.0f / 3.1415926536; + roll_acc *= 180.0f / 3.1415926536; + + // 积分得到Yaw角 + yaw += gyroZ * YAW_COMPENSATION_FACTOR; + + // 陀螺仪积分（需初始化时用加速度计角度赋值） + static uint8_t is_first_run = 1; + if (is_first_run) { + pitch = pitch_acc; + roll = roll_acc; + is_first_run = 0; + } else { + pitch += gyroY * dt; // 陀螺仪Y轴对应俯仰角速度 + roll += gyroX * dt; // 陀螺仪X轴对应横滚角速度 + } + + // 短期修正：当设备静止时，若加速度计稳定，可重置漂移（需自定义条件） + float accelMagnitude = sqrtf( + data.AccelX * data.AccelX + + data.AccelY * data.AccelY + + data.AccelZ * data.AccelZ + ); + if (fabs(accelMagnitude - 1.0f) < 0.1f) { // 加速度接近1g时认为静止 + // 可在此处重置Yaw或补偿漂移（例如：yaw *= 0.99;） + } + + // 互补滤波（α=0.96，权重陀螺仪） + float alpha = 0.96f; + pitch = alpha * pitch + (1 - alpha) * pitch_acc; + roll = alpha * roll + (1 - alpha) * roll_acc; +} + + +/** + * @brief 更新偏航角 + * @param None + * @retval None + */ +void MPU6050_UpdateYaw() { + + float gyro_sensitivity; + switch (gyro_cfg) { + case 0: gyro_sensitivity = 131.0f; break; // ±250dps + case 1: gyro_sensitivity = 65.5f; break; // ±500dps + case 2: gyro_sensitivity = 32.8f; break; // ±1000dps + case 3: gyro_sensitivity = 16.4f; break; // ±2000dps + default: gyro_sensitivity = 131.0f; break; + } + + MPU6050_Raw data = MPU6050_GetRawData(); + float gyroZ = (data.GyroZ - gyroZ_offset) / gyro_sensitivity; + + yaw += gyroZ * YAW_COMPENSATION_FACTOR; +} + + +/** + * @brief 获取俯仰角 + * @param None + * @retval pitch + */ +float MPU6050_GetPitch() { + return pitch; +} + +/** + * @brief 获取横滚角 + * @param None + * @retval roll + */ +float MPU6050_GetRoll() { + return roll; +} + +/** + * @brief 获取偏航角 + * @param None + * @retval yaw + */ +float MPU6050_GetYaw() { + return yaw; +} -- Gitee From 4fe521d68d2c2d0986ce2f490e4fa2c2e17f8803 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:34:38 +0000 Subject: [PATCH 11/12] TCS34725 basic lib Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- TCS34725/Inc/tcs34725.h | 22 ++++++++++++++++ TCS34725/Src/tcs34725.c | 56 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 TCS34725/Inc/tcs34725.h create mode 100644 TCS34725/Src/tcs34725.c diff --git a/TCS34725/Inc/tcs34725.h b/TCS34725/Inc/tcs34725.h new file mode 100644 index 0000000..8c83242 --- /dev/null +++ b/TCS34725/Inc/tcs34725.h @@ -0,0 +1,22 @@ +#include "i2c.h" + +typedef struct { + uint16_t clear; + uint16_t red; + uint16_t green; + uint16_t blue; +} TCS34725_RawData; + +#define COMMAND_BIT 0x80 + +#define TCS34725_ADDR (0x29 << 1) +#define TCS34725_ENABLE_REG (COMMAND_BIT | 0x00) +#define TCS34725_ATIME_REG (COMMAND_BIT | 0x01) +#define TCS34725_CONTROL_REG (COMMAND_BIT | 0x0F) +#define TCS34725_ID_REG (COMMAND_BIT | 0x12) +#define TCS34725_STATUS_REG (COMMAND_BIT | 0x13) +#define TCS34725_CDATA_REG (COMMAND_BIT | 0x14) + +uint8_t TCS34725_Init(I2C_HandleTypeDef *hi2c, uint8_t atime, uint8_t gain); +TCS34725_RawData TCS34725_GetRawData(I2C_HandleTypeDef *hi2c); +uint8_t TCS34725_DataReady(I2C_HandleTypeDef *hi2c); diff --git a/TCS34725/Src/tcs34725.c b/TCS34725/Src/tcs34725.c new file mode 100644 index 0000000..44324ab --- /dev/null +++ b/TCS34725/Src/tcs34725.c @@ -0,0 +1,56 @@ +#include "tcs34725.h" + +/** + * @brief 初始化TCS34725传感器 + * @param hi2c: I2C句柄 + * @param atime: 设置积分时间 (0x00 - 0xFF, 越大时间越短, 但精度越低) int.time = (256 - atime) × 2.4ms + * @param gain: 设置增益 (0x00 - 1x, 0x01 - 4x, 0x02 - 16x, 0x03 - 60x) + * @retval 1: 初始化成功, 0: 初始化失败 + */ +uint8_t TCS34725_Init(I2C_HandleTypeDef *hi2c, uint8_t atime, uint8_t gain) { + uint8_t device_id; + HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_ID_REG, 1, &device_id, 1, 100); + + if (device_id != 0x44 && device_id != 0x4D) { return 0; } + + HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_ATIME_REG, 1, &atime, 1, 100); + + HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_CONTROL_REG, 1, &gain, 1, 100); + + uint8_t enable = 0x03; // PON = 1, AEN = 1 + HAL_I2C_Mem_Write(hi2c, TCS34725_ADDR, TCS34725_ENABLE_REG, 1, &enable, 1, 100); + + HAL_Delay(50); + return 1; +} + +/** + * @brief 获取TCS34725的 RGBC + * @param data: 存储 RGBC 数据 + * @retval None + */ +TCS34725_RawData TCS34725_GetRawData(I2C_HandleTypeDef *hi2c) { + uint8_t raw_data[8]; + TCS34725_RawData data; + + HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_CDATA_REG, 1, raw_data, 8, 100); + + // 组合高低字节，数据低字节在前 + data.clear = (raw_data[1] << 8) | raw_data[0]; + data.red = (raw_data[3] << 8) | raw_data[2]; + data.green = (raw_data[5] << 8) | raw_data[4]; + data.blue = (raw_data[7] << 8) | raw_data[6]; + + return data; +} + +/** + * @brief 检查TCS34725数据是否准备好 + * @param hi2c: I2C句柄 + * @retval 1: 数据准备好, 0: 数据未准备好 + */ +uint8_t TCS34725_DataReady(I2C_HandleTypeDef *hi2c) { + uint8_t status; + HAL_I2C_Mem_Read(hi2c, TCS34725_ADDR, TCS34725_STATUS_REG, 1, &status, 1, 100); + return (status & 0x01); +} -- Gitee From a62521a9a0b820a6bf571b6223a50ad670b28bb7 Mon Sep 17 00:00:00 2001 From: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> Date: Mon, 12 May 2025 14:37:06 +0000 Subject: [PATCH 12/12] add TCS34725/readme.md. Signed-off-by: WillliTourt <15819814+willlitourt@user.noreply.gitee.com> --- TCS34725/readme.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 TCS34725/readme.md diff --git a/TCS34725/readme.md b/TCS34725/readme.md new file mode 100644 index 0000000..5dd282b --- /dev/null +++ b/TCS34725/readme.md @@ -0,0 +1 @@ +## TCS34725颜色传感器模块的基础库 仅用于读出原始RGBC数据 不包含RGB转RGB888/RGB565 \ No newline at end of file -- Gitee