105 lines
5.3 KiB
C++
105 lines
5.3 KiB
C++
|
||
/*
|
||
* FIXME: 1. 传感器数据校准
|
||
2. 可采用卡尔曼滤波
|
||
|
||
FIXME: 采用类进行封装，因为可能需要一对
|
||
*/
|
||
#include <cmath>
|
||
#include <cstdio>
|
||
#include <cstdint>
|
||
#include <unistd.h>
|
||
#include "cn/中文化.hpp"
|
||
#include "Dashun_linux.hpp"
|
||
|
||
静态的 预求值的 无号8位整 MPU6050_默认地址 = 0x68;
|
||
静态的 预求值的 无号8位整 MPU6050_电源管理寄存器1 = 0x6B;
|
||
静态的 预求值的 无号8位整 MPU6050_电源管理_非睡眠模式 = 0x00;
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪配置寄存器 = 0x1B;
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪量程_正负500度每秒 = 0x08;
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪横轴数据_高字节 = 0x43; /* x */
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪横轴数据_低字节 = 0x44;
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪纵轴数据_高字节 = 0x45; /* y */
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪纵轴数据_低字节 = 0x46;
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪竖轴数据_高字节 = 0x47; /* z */
|
||
静态的 预求值的 无号8位整 MPU6050_陀螺仪竖轴数据_低字节 = 0x48;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计配置寄存器 = 0x1C;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计量程_正负8g = 0x10;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计横轴数据_高字节 = 0x3B;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计横轴数据_低字节 = 0x3C;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计纵轴数据_高字节 = 0x3D;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计纵轴数据_低字节 = 0x3E;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计竖轴数据_高字节 = 0x3F;
|
||
静态的 预求值的 无号8位整 MPU6050_加速度计竖轴数据_低字节 = 0x40;
|
||
静态的 逻辑型 启动标志 = 假;
|
||
|
||
逻辑型 MPU6050_检查() {
|
||
I2C_获取锁();
|
||
布尔 临时 = 启动标志;
|
||
I2C_释放锁();
|
||
返回 临时;
|
||
}
|
||
|
||
/* 初始化 MPU6050 */
|
||
空类型 MPU6050_初始化() {
|
||
I2C_获取锁();
|
||
I2C_置地址(MPU6050_默认地址);
|
||
I2C_写寄存器(MPU6050_电源管理寄存器1, MPU6050_电源管理_非睡眠模式); /* 退出睡眠 */
|
||
I2C_写寄存器(MPU6050_陀螺仪配置寄存器, MPU6050_陀螺仪量程_正负500度每秒); /* ±500°/s */
|
||
I2C_写寄存器(MPU6050_加速度计配置寄存器, MPU6050_加速度计量程_正负8g); /* ±8g */
|
||
启动标志 = 真;
|
||
I2C_释放锁();
|
||
usleep(100000); /* 等待稳定 */
|
||
}
|
||
|
||
/* 读取 16 位传感器数据（带符号） */
|
||
静态的 有号16位整 读16比特寄存器(无号8位整 寄存器高位, 无号8位整 寄存器低位) {
|
||
返回 (I2C_读寄存器(寄存器高位) << 8) | I2C_读寄存器(寄存器低位);
|
||
}
|
||
|
||
/* 获取原始MPU6050传感器数据 */
|
||
MPU6050_传感器数据 MPU6050_读传感器() {
|
||
MPU6050_传感器数据 数据;
|
||
I2C_获取锁();
|
||
I2C_置地址(MPU6050_默认地址);
|
||
数据.加速度计_横 = 读16比特寄存器(MPU6050_加速度计横轴数据_高字节, MPU6050_加速度计横轴数据_低字节);
|
||
数据.加速度计_纵 = 读16比特寄存器(MPU6050_加速度计纵轴数据_高字节, MPU6050_加速度计纵轴数据_低字节);
|
||
数据.加速度计_竖 = 读16比特寄存器(MPU6050_加速度计竖轴数据_高字节, MPU6050_加速度计竖轴数据_低字节);
|
||
数据.陀螺仪_横 = 读16比特寄存器(MPU6050_陀螺仪横轴数据_高字节, MPU6050_陀螺仪横轴数据_低字节);
|
||
数据.陀螺仪_纵 = 读16比特寄存器(MPU6050_陀螺仪纵轴数据_高字节, MPU6050_陀螺仪纵轴数据_低字节);
|
||
数据.陀螺仪_竖 = 读16比特寄存器(MPU6050_陀螺仪竖轴数据_高字节, MPU6050_陀螺仪竖轴数据_低字节);
|
||
I2C_释放锁();
|
||
返回 数据;
|
||
}
|
||
|
||
/* FIXME: 当设备剧烈运动时（如加速度远大于重力），加速度计数据会包含运动加速度，此时角度计算可能不准确。实际应用中需结合陀螺仪数据（如互补滤波或卡尔曼滤波）来提高稳定性。 */
|
||
/* FIXME: 适合静态测量或学习用途。如果需要实际应用，建议添加传感器融合算法 */
|
||
空类型 MPU6050_计算角度(常量 MPU6050_传感器数据& 数据, 单精度小数& 俯仰角, 单精度小数& 翻滚角) {
|
||
/* - ±2g → 16384 LSB/g
|
||
- ±4g → 8192 LSB/g
|
||
- ±8g → 4096 LSB/g
|
||
- ±16g → 2048 LSB/g */
|
||
常量 单精度小数 比例 = 4096.0; /* 加速度数据转重力单位（±8g 量程 4096 LSB/g） */
|
||
单精度小数 横角 = 数据.加速度计_横 / 比例;
|
||
单精度小数 纵角 = 数据.加速度计_纵 / 比例;
|
||
单精度小数 竖角 = 数据.加速度计_竖 / 比例;
|
||
|
||
/* 计算俯仰角(Pitch)和翻滚角(Roll) */
|
||
俯仰角 = atan2(-横角, sqrt(纵角 * 纵角 + 竖角 * 竖角)) * 180.0 / M_PI;
|
||
翻滚角 = atan2(纵角, 竖角) * 180.0 / M_PI;
|
||
}
|
||
|
||
#ifdef _TEST
|
||
/* TEST MAIN */
|
||
整数 主函数() {
|
||
I2C_初始化();
|
||
MPU6050_初始化();
|
||
判断循环 (1) {
|
||
MPU6050_传感器数据 数据 = MPU6050_读传感器();
|
||
单精度小数 俯仰角, 翻滚角;
|
||
MPU6050_计算角度(数据, 俯仰角, 翻滚角);
|
||
printf("俯仰角: %.2f\t翻滚角: %.2f\n");
|
||
}
|
||
}
|
||
#endif
|