MPU6050读取航向角代码实现
时间: 2025-03-03 16:14:34 浏览: 78
### MPU6050 读取航向角 示例代码
为了从 MPU6050 中获取航向角(Yaw 角),通常需要使用内置的数字运动处理器(DMP),该组件能够处理原始传感器数据并输出四元数,之后再转换成欧拉角来获得角度信息[^3]。
下面是一个基于 Arduino 平台的例子,用于展示如何配置 MPU6050 和读取 Yaw 航向角:
```cpp
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// Class declaration for the MPU6050
MPU6050 mpu;
#define LED_PIN 13 // (Arduino is 13, Teensy is 6, etc.)
bool blinkState = false;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q; // [w, x, y, z]
VectorFloat gravity; // [x, y, z]
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
Wire.begin();
Serial.begin(9600);
pinMode(LED_PIN, OUTPUT);
// Initialize MPU6050 and test connection.
mpu.initialize();
if (!mpu.testConnection())
Serial.println("MPU6050 connection failed");
else
Serial.println("MPU6050 connection successful");
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
if (devStatus != 0) {
Serial.print(F("DMP Initialization failed"));
}
else {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.InterruptMode(true);
attachInterrupt(digitalPinToInterrupt(mpu.getIntPin()), dmpDataReady, RISING);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
}
}
void loop()
{
if (mpuInterrupt && fifoCount < packetSize)
{
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024)
mpu.resetFIFO();
else if (fifoCount >= packetSize)
{
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
float yaw = atan2(gravity.y, gravity.z)*RAD_TO_DEG;
Serial.print("Yaw Angle: ");
Serial.print(yaw);
Serial.println(" deg");
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
mpuInterrupt = false;
}
}
```
这段程序展示了 MPU6050 的初始化过程以及如何通过 DMP 获取四元数值,并将其转化为重力矢量以计算出 Yaw 角度。注意,在实际应用中可能还需要考虑更多细节比如偏移校准等操作[^1]。
阅读全文
相关推荐

















