81 lines
2.1 KiB
C++
81 lines
2.1 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;
|
|||
|
|
|
|||
|
|
// ===== 初始化 =====
|
|||
|
|
void imu_setup() {
|
|||
|
|
Serial.begin(115200);
|
|||
|
|
Wire.begin(21, 22);
|
|||
|
|
|
|||
|
|
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 读取，返回 YPR（角度制）=====
|
|||
|
|
YPR dmp() {
|
|||
|
|
YPR out = {0, 0, 0};
|
|||
|
|
if (!dmpReady) return out;
|
|||
|
|
|
|||
|
|
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
|
|||
|
|
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
|||
|
|
mpu.dmpGetGravity(&gravity, &q);
|
|||
|
|
float ypr[3];
|
|||
|
|
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
|||
|
|
|
|||
|
|
out.yaw = ypr[0] * 180.0f / M_PI;
|
|||
|
|
out.pitch = ypr[1] * 180.0f / M_PI;
|
|||
|
|
out.roll = ypr[2] * 180.0f / M_PI;
|
|||
|
|
}
|
|||
|
|
// Serial.print("Yaw: "); Serial.print(a.yaw);
|
|||
|
|
// Serial.print(" Pitch: "); Serial.print(a.pitch);
|
|||
|
|
// Serial.print(" Roll: "); Serial.println(a.roll);
|
|||
|
|
return out;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== 原始数据读取，返回 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;
|
|||
|
|
}
|