99 lines
2.4 KiB
C++
99 lines
2.4 KiB
C++
#include "I2Cdev.h"
|
|
#include "MPU6050_6Axis_MotionApps20.h"
|
|
#include "Wire.h"
|
|
#include <MPU6050.h>
|
|
#include "imu.h"
|
|
|
|
MPU6050 mpu;
|
|
bool dmpReady = false;
|
|
uint8_t fifoBuffer[64];
|
|
|
|
Quaternion q;
|
|
VectorFloat gravity;
|
|
GravityVector lastGravity = {0, 0, 0};
|
|
|
|
// ===== 初始化 =====
|
|
void imu_setup() {
|
|
Serial.begin(115200);
|
|
Wire.begin(21, 22);
|
|
Wire.setClock(400000);
|
|
Wire.setTimeOut(5);
|
|
|
|
mpu.initialize();
|
|
if (!mpu.testConnection()) {
|
|
Serial.println("MPU6050 连接失败");
|
|
while (1);
|
|
}
|
|
|
|
uint8_t devStatus = mpu.dmpInitialize();
|
|
if (devStatus == 0) {
|
|
mpu.CalibrateAccel(6);
|
|
mpu.CalibrateGyro(6);
|
|
mpu.setDMPEnabled(true);
|
|
dmpReady = true;
|
|
Serial.println("DMP 就绪");
|
|
} else {
|
|
Serial.print("DMP 初始化失败，错误码: ");
|
|
Serial.println(devStatus);
|
|
}
|
|
}
|
|
|
|
// ===== DMP 读取，返回单位重力向量在 XYZ 轴上的投影 =====
|
|
|
|
GravityVector dmp() {
|
|
if (!dmpReady) return lastGravity;
|
|
|
|
const uint16_t packetSize = mpu.dmpGetFIFOPacketSize();
|
|
if (packetSize == 0 || packetSize > sizeof(fifoBuffer)) {
|
|
return lastGravity;
|
|
}
|
|
|
|
uint16_t fifoCount = mpu.getFIFOCount();
|
|
if (fifoCount >= 1024) {
|
|
mpu.resetFIFO();
|
|
return lastGravity;
|
|
}
|
|
|
|
if (fifoCount >= packetSize) {
|
|
if (fifoCount > packetSize * 5U) {
|
|
mpu.resetFIFO();
|
|
return lastGravity;
|
|
}
|
|
|
|
while (fifoCount >= packetSize) {
|
|
mpu.getFIFOBytes(fifoBuffer, static_cast<uint8_t>(packetSize));
|
|
fifoCount -= packetSize;
|
|
}
|
|
|
|
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
|
mpu.dmpGetGravity(&gravity, &q);
|
|
lastGravity.x = gravity.x;
|
|
lastGravity.y = gravity.y;
|
|
lastGravity.z = gravity.z;
|
|
}
|
|
return lastGravity;
|
|
}
|
|
|
|
// ===== 原始数据读取，返回 RawData =====
|
|
RawData raw() {
|
|
RawData r;
|
|
mpu.getMotion6(&r.ax, &r.ay, &r.az, &r.gx, &r.gy, &r.gz);
|
|
|
|
// ±2g 量程下 1g = 16384 LSB
|
|
// r.ax_g = r.ax / 16384.0f;
|
|
// r.ay_g = r.ay / 16384.0f;
|
|
// r.az_g = r.az / 16384.0f;
|
|
|
|
// // ±250°/s 量程下 1°/s = 131 LSB
|
|
// r.gx_dps = r.gx / 131.0f;
|
|
// r.gy_dps = r.gy / 131.0f;
|
|
// r.gz_dps = r.gz / 131.0f;
|
|
// Serial.print("Accel(g): X="); Serial.print(r.ax_g, 3);
|
|
// Serial.print(" Y="); Serial.print(r.ay_g, 3);
|
|
// Serial.print(" Z="); Serial.print(r.az_g, 3);
|
|
// Serial.print(" | Gyro(°/s): X="); Serial.print(r.gx_dps, 2);
|
|
// Serial.print(" Y="); Serial.print(r.gy_dps, 2);
|
|
// Serial.print(" Z="); Serial.println(r.gz_dps, 2);
|
|
|
|
return r;
|
|
} |