MPU6050在初始化后不久可用,运行稳定。但是同样的文件上电使用长时间之后会读取不出数据,也没有初始化失败。用别的库初始化一下就又能用了。
主控esp32s3总线上有ssd1306。用的硬件I2C接口1和0
这是两个关键库:
// mpu6050.cpp
#include "readreg.h"
#include <string.h>
smalli2c l2c(I2C_NUM_1,MPU6050_I2C_ADDR);
MPU6050::MPU6050() :
accel_range_(ACCEL_FS_2G),
gyro_range_(GYRO_FS_250DPS),
accel_sensitivity_(16384.0f),
gyro_sensitivity_(131.0f) {
}
MPU6050::~MPU6050() {
i2c_driver_delete(i2c_num_);
}
esp_err_t MPU6050::init(){
esp_err_t ret = l2c.begin();
if (ret != ESP_OK) {
return ret;
}
delay(500);
ret = reset();
if (ret != ESP_OK) {
ESP_LOGE("MPU6050", "Failed to reset MPU6050");
return ret;
}
delay(500);
// 唤醒MPU6050
ret = wakeup();
if (ret != ESP_OK) {
ESP_LOGE("MPU6050", "Failed to wakeup MPU6050");
return ret;
}
delay(500);
// 设置默认量程
ret = set_accel_range(accel_range_);
if (ret != ESP_OK) {
ESP_LOGE("MPU6050", "Failed to set accelerometer range");
return ret;
}
ret = set_gyro_range(gyro_range_);
if (ret != ESP_OK) {
ESP_LOGE("MPU6050", "Failed to set gyroscope range");
return ret;
}
// 设置采样率分频器
ret = set_sample_rate(0); // 1kHz
if (ret != ESP_OK) {
ESP_LOGE("MPU6050", "Failed to set sample rate");
return ret;
}
ESP_LOGI("MPU6050", "MPU6050 initialized successfully");
return ESP_OK;
}
esp_err_t MPU6050::begin() {
return l2c.begin();
}
esp_err_t MPU6050::reset() {
return l2c.write_byte(MPU6050_REG_PWR_MGMT_1, 0x80); // 设置复位位
}
esp_err_t MPU6050::wakeup() {
// 清除休眠位,选择X轴陀螺仪作为时钟源
return l2c.write_byte(MPU6050_REG_PWR_MGMT_1, 0x01);
}
esp_err_t MPU6050::sleep() {
// 设置休眠位
return l2c.write_byte(MPU6050_REG_PWR_MGMT_1, 0x40);
}
void MPU6050::update_sensitivity() {
// 更新加速度计量程灵敏度(LSB/g)
switch (accel_range_) {
case ACCEL_FS_2G:
accel_sensitivity_ = 16384.0f;
break;
case ACCEL_FS_4G:
accel_sensitivity_ = 8192.0f;
break;
case ACCEL_FS_8G:
accel_sensitivity_ = 4096.0f;
break;
case ACCEL_FS_16G:
accel_sensitivity_ = 2048.0f;
break;
}
// 更新陀螺仪量程灵敏度(LSB/°/s)
switch (gyro_range_) {
case GYRO_FS_250DPS:
gyro_sensitivity_ = 131.0f;
break;
case GYRO_FS_500DPS:
gyro_sensitivity_ = 65.5f;
break;
case GYRO_FS_1000DPS:
gyro_sensitivity_ = 32.8f;
break;
case GYRO_FS_2000DPS:
gyro_sensitivity_ = 16.4f;
break;
}
}
esp_err_t MPU6050::set_accel_range(accel_fs_t range) {
accel_range_ = range;
update_sensitivity();
return l2c.write_byte(MPU6050_REG_ACCEL_CONFIG, range << 3);
}
esp_err_t MPU6050::set_gyro_range(gyro_fs_t range) {
gyro_range_ = range;
update_sensitivity();
return l2c.write_byte(MPU6050_REG_GYRO_CONFIG, range << 3);
}
esp_err_t MPU6050::set_sample_rate(uint8_t rate) {
// 采样率 = 陀螺仪输出频率 / (1 + SMPLRT_DIV)
return l2c.write_byte(MPU6050_REG_SMPLRT_DIV, rate);
}
esp_err_t MPU6050::get_raw_data(mpu6050_data_t* data) {
if (data == nullptr) return ESP_ERR_INVALID_ARG;
uint8_t buffer[14];
esp_err_t ret = l2c.read_bytes(MPU6050_REG_ACCEL_XOUT_H, buffer, 14);
if (ret == ESP_OK) {
data->accel_x = (buffer[0] << 8) | buffer[1];
data->accel_y = (buffer[2] << 8) | buffer[3];
data->accel_z = (buffer[4] << 8) | buffer[5];
data->temp = (buffer[6] << 8) | buffer[7];
data->gyro_x = (buffer[8] << 8) | buffer[9];
data->gyro_y = (buffer[10] << 8) | buffer[11];
data->gyro_z = (buffer[12] << 8) | buffer[13];
}
return ret;
}
esp_err_t MPU6050::get_accel_raw(int16_t* x, int16_t* y, int16_t* z) {
uint8_t buffer[6];
esp_err_t ret = l2c.read_bytes(MPU6050_REG_ACCEL_XOUT_H, buffer, 6);
if (ret == ESP_OK) {
if (x) *x = (buffer[0] << 8) | buffer[1];
if (y) *y = (buffer[2] << 8) | buffer[3];
if (z) *z = (buffer[4] << 8) | buffer[5];
}
return ret;
}
esp_err_t MPU6050::get_gyro_raw(int16_t* x, int16_t* y, int16_t* z) {
uint8_t buffer[6];
esp_err_t ret = l2c.read_bytes(MPU6050_REG_GYRO_XOUT_H, buffer, 6);
if (ret == ESP_OK) {
if (x) *x = (buffer[0] << 8) | buffer[1];
if (y) *y = (buffer[2] << 8) | buffer[3];
if (z) *z = (buffer[4] << 8) | buffer[5];
}
return ret;
}
esp_err_t MPU6050::get_temp_raw(int16_t* temp) {
return l2c.read_word(MPU6050_REG_TEMP_OUT_H, temp);
}
float MPU6050::get_temp_celsius() {
int16_t temp_raw;
if (get_temp_raw(&temp_raw) != ESP_OK) {
return 0.0f;
}
return (temp_raw / 340.0f) + 36.53f;
}
esp_err_t MPU6050::self_test() {
// 自检功能
uint8_t self_test[6];
esp_err_t ret = l2c.read_bytes(0x0D, self_test, 6);
if (ret == ESP_OK) {
// 检查自检返回值
uint8_t factory_trim = (self_test[0] & 0x1F) |
(self_test[1] & 0x1F) |
(self_test[2] & 0x1F) |
(self_test[3] & 0x1F) |
(self_test[4] & 0x1F) |
(self_test[5] & 0x1F);
if (factory_trim != 0) {
ESP_LOGI("MPU6050", "Self test passed");
} else {
ESP_LOGW("MPU6050", "Self test failed");
ret = ESP_FAIL;
}
}
return ret;
}
uint8_t MPU6050::who_am_i() {
uint8_t id = 0;
l2c.read_byte(MPU6050_REG_WHO_AM_I, &id);
return id;
}
//开始数据处理
// 坐标转换函数
// 假设芯片坐标系逆时针旋转45°安装
void MPU6050::transform_to_world_coordinates(const mpu6050_data_t* chip_data, world_data_t* world_data) {
// 将原始数据转换为浮点数(根据量程)
// 假设GYRO_FS_2000DPS: 32768 = 2000°/s
float gyro_scale = 2000.0f / 32768.0f; // dps/LSB
float accel_scale = 2.0f / 32768.0f; // 假设±2g,实际根据你的设置调整
// 将原始数据转换为物理量(可选,也可以直接对原始值进行旋转)
float gx_chip = chip_data->gyro_x * gyro_scale;
float gy_chip = chip_data->gyro_y * gyro_scale;
float gz_chip = chip_data->gyro_z * gyro_scale;
// 陀螺仪转换(逆时针45°安装的补偿)
world_data->gyro_x = COS_45 * gx_chip - SIN_45 * gy_chip; // 注意:这是逆变换
world_data->gyro_y = SIN_45 * gx_chip + COS_45 * gy_chip;
world_data->gyro_z = gz_chip;
// 如果有加速度计数据,同样处理
// 假设你的数据结构中有accel_x, accel_y, accel_z
// float ax_chip = chip_data->accel_x * accel_scale;
// float ay_chip = chip_data->accel_y * accel_scale;
// float az_chip = chip_data->accel_z * accel_scale;
// world_data->accel_x = COS_45 * ax_chip - SIN_45 * ay_chip;
// world_data->accel_y = SIN_45 * ax_chip + COS_45 * ay_chip;
// world_data->accel_z = az_chip;
}
// 备选:如果安装是顺时针45°,使用这个函数
void MPU6050::transform_to_world_coordinates_cw(const mpu6050_data_t* chip_data, world_data_t* world_data) {
float gyro_scale = 2000.0f / 32768.0f;
float gx_chip = chip_data->gyro_x * gyro_scale;
float gy_chip = chip_data->gyro_y * gyro_scale;
// 顺时针45°安装的补偿
world_data->gyro_x = COS_45 * gx_chip + SIN_45 * gy_chip;
world_data->gyro_y = -SIN_45 * gx_chip + COS_45 * gy_chip;
world_data->gyro_z = chip_data->gyro_z * gyro_scale;
}
// 直接在原始数据上进行转换(更高效)有点不对啊
void MPU6050::transform_raw_data(const mpu6050_data_t* chip_data, world_data_t* world_data) {
// 直接对原始数据进行旋转,最后再统一缩放
world_data->gyro_x = COS_45 * chip_data->gyro_x - SIN_45 * chip_data->gyro_y;
world_data->gyro_y = SIN_45 * chip_data->gyro_x + COS_45 * chip_data->gyro_y;
world_data->gyro_z = chip_data->gyro_z;
// 注:这样处理的前提是X和Y轴使用相同的量程
}
#include "smalli2c.h"
#include "driver/i2c.h"
#include "esp_log.h"
smalli2c::smalli2c(i2c_port_t i2c_num, uint8_t device_addr, u8_t sda_pin, u8_t scl_pin, uint32_t i2c_freq)
: i2c_num_(i2c_num), device_addr_(device_addr), sda_pin_(sda_pin), scl_pin_(scl_pin), i2c_freq_(i2c_freq) {
}
smalli2c::~smalli2c() {
}
bool smalli2c::begin() {
i2c_config_t conf = {
.mode = I2C_MODE_MASTER,
.sda_io_num = sda_pin_,
.scl_io_num = scl_pin_,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
.master = {
.clk_speed = i2c_freq_
},
.clk_flags = 0,
};
esp_err_t ret = i2c_param_config(i2c_num_, &conf);
if (ret != ESP_OK) {
return false;
}
ret = i2c_driver_install(i2c_num_, conf.mode, 0, 0, 0);
return (ret == ESP_OK);
}
esp_err_t smalli2c::write_byte(uint8_t reg, uint8_t data) {
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (device_addr_ << 1) | I2C_MASTER_WRITE, true);
i2c_master_write_byte(cmd, reg, true);
i2c_master_write_byte(cmd, data, true);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(i2c_num_, cmd, pdMS_TO_TICKS(1000));
i2c_cmd_link_delete(cmd);
if (ret != ESP_OK) {
ESP_LOGE("MPU6050", "Write byte failed: %s", esp_err_to_name(ret));
}
return ret;
}
esp_err_t smalli2c::writePackage(uint8_t reg,uint8_t *data, size_t length) {
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (device_addr_ << 1) | I2C_MASTER_WRITE, true);
i2c_master_write_byte(cmd,reg,true);
i2c_master_write(cmd, data, length, true);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(i2c_num_, cmd, 1000 / portTICK_PERIOD_MS);
i2c_cmd_link_delete(cmd);
return ret;
}
esp_err_t smalli2c::read_byte(uint8_t reg, uint8_t* value) {
if (value == nullptr) {
return ESP_ERR_INVALID_ARG;
}
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (device_addr_ << 1) | I2C_MASTER_WRITE, true);
i2c_master_write_byte(cmd, reg, true);
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (device_addr_ << 1) | I2C_MASTER_READ, true);
i2c_master_read_byte(cmd, value, I2C_MASTER_NACK);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(i2c_num_, cmd, pdMS_TO_TICKS(1000));
i2c_cmd_link_delete(cmd);
return ret;
}
esp_err_t smalli2c::read_bytes(uint8_t reg, uint8_t *buffer, size_t length) {
if (buffer == nullptr || length == 0) {
return ESP_ERR_INVALID_ARG;
}
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (device_addr_ << 1) | I2C_MASTER_WRITE, true);
i2c_master_write_byte(cmd, reg, true);
i2c_master_start(cmd);
i2c_master_write_byte(cmd, (device_addr_ << 1) | I2C_MASTER_READ, true);
if (length > 1) {
i2c_master_read(cmd, buffer, length - 1, I2C_MASTER_ACK);
}
i2c_master_read_byte(cmd, buffer + length - 1, I2C_MASTER_NACK);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(i2c_num_, cmd, pdMS_TO_TICKS(1000));
i2c_cmd_link_delete(cmd);
return ret;
}
esp_err_t smalli2c::read_word(uint8_t reg, int16_t* value) {
if (value == nullptr) {
return ESP_ERR_INVALID_ARG;
}
uint8_t buffer[2];
esp_err_t ret = read_bytes(reg, buffer, 2);
if (ret == ESP_OK) {
*value = (buffer[0] << 8) | buffer[1];
}
return ret;
}
这是实例::
#include <Arduino.h>
#include <readreg.h> // 假设这是你的MPU6050库头文件
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
MPU6050 mpu;
mpu6050_data_t data;
world_data_t world_data;
// 坐标转换函数
// 假设芯片坐标系逆时针旋转45°安装
void transform_to_world_coordinates(const mpu6050_data_t* chip_data, world_data_t* world_data) {
// 将原始数据转换为浮点数(根据量程)
// 假设GYRO_FS_2000DPS: 32768 = 2000°/s
float gyro_scale = 2000.0f / 32768.0f; // dps/LSB
float accel_scale = 2.0f / 32768.0f; // 假设±2g,实际根据你的设置调整
// 将原始数据转换为物理量(可选,也可以直接对原始值进行旋转)
float gx_chip = chip_data->gyro_x * gyro_scale;
float gy_chip = chip_data->gyro_y * gyro_scale;
float gz_chip = chip_data->gyro_z * gyro_scale;
// 陀螺仪转换(逆时针45°安装的补偿)
world_data->gyro_x = COS_45 * gx_chip - SIN_45 * gy_chip; // 注意:这是逆变换
world_data->gyro_y = SIN_45 * gx_chip + COS_45 * gy_chip;
world_data->gyro_z = gz_chip;
// 如果有加速度计数据,同样处理
// 假设你的数据结构中有accel_x, accel_y, accel_z
// float ax_chip = chip_data->accel_x * accel_scale;
// float ay_chip = chip_data->accel_y * accel_scale;
// float az_chip = chip_data->accel_z * accel_scale;
// world_data->accel_x = COS_45 * ax_chip - SIN_45 * ay_chip;
// world_data->accel_y = SIN_45 * ax_chip + COS_45 * ay_chip;
// world_data->accel_z = az_chip;
}
// 备选:如果安装是顺时针45°,使用这个函数
void transform_to_world_coordinates_cw(const mpu6050_data_t* chip_data, world_data_t* world_data) {
float gyro_scale = 2000.0f / 32768.0f;
float gx_chip = chip_data->gyro_x * gyro_scale;
float gy_chip = chip_data->gyro_y * gyro_scale;
// 顺时针45°安装的补偿
world_data->gyro_x = COS_45 * gx_chip + SIN_45 * gy_chip;
world_data->gyro_y = -SIN_45 * gx_chip + COS_45 * gy_chip;
world_data->gyro_z = chip_data->gyro_z * gyro_scale;
}
// 直接在原始数据上进行转换(更高效)有点不对啊
void transform_raw_data(const mpu6050_data_t* chip_data, world_data_t* world_data) {
// 直接对原始数据进行旋转,最后再统一缩放
world_data->gyro_x = COS_45 * chip_data->gyro_x - SIN_45 * chip_data->gyro_y;
world_data->gyro_y = SIN_45 * chip_data->gyro_x + COS_45 * chip_data->gyro_y;
world_data->gyro_z = chip_data->gyro_z;
// 注:这样处理的前提是X和Y轴使用相同的量程
}
void setup() {
Serial.begin(115200);
mpu.begin();
mpu.set_gyro_range(GYRO_FS_2000DPS);
// 根据你的需要设置加速度计量程
// mpu.set_accel_range(ACCEL_FS_2G);
uint8_t id = mpu.who_am_i();
//calibrate_installation();
Serial.print("MPU6050 ID: 0x");
Serial.println(id, HEX);
Serial.println("开始读取数据,转换到世界坐标系...");
}
float delay_s=0.2;
void loop() {
if (mpu.get_raw_data(&data) == ESP_OK) {
// 转换到世界坐标系
transform_to_world_coordinates(&data, &world_data);
// 打印转换后的世界坐标系数据
printf(": X=%6.1f°, Y=%6.1f, Z=%6.1f°/delay_s\n",
world_data.gyro_x*delay_s, world_data.gyro_y*delay_s, world_data.gyro_z*delay_s);
Serial.println();
}
delay(delay_s*1000);
}