2. 加速度计倾角解算与可视化

任务说明

本案例读取三轴加速度计（如 MPU6050 的加速度部分）的原始数据，用重力分量与三角函数解算俯仰角 pitch 和横滚角 roll，并通过串口以固定周期实时输出。核心公式：

• pitch = atan2(-ax, sqrt(ay² + az²))
• roll = atan2(ay, az)

加速度计静止时，重力矢量在三轴上的投影决定姿态角；atan2 相比 atan 能正确处理象限并避免除零。下面给出 Arduino 平台的最小完整实现，串口输出 CSV 格式便于绘图。

数据流与解算流程

完整代码

以下代码在 Arduino UNO 上运行，通过 I2C 读取 MPU6050 加速度寄存器（0x3B 起 6 字节），每 100ms 解算并输出一次。若使用其他加速度计，只需替换 readAccel() 中的读取逻辑，保持输出单位为 g。

#include <Wire.h>
#include <math.h>

const uint8_t MPU_ADDR = 0x68;
const float   ACCEL_SCALE = 16384.0f;  // ±2g 量程, LSB/g

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);   // PWR_MGMT_1
  Wire.write(0x00);   // 唤醒
  Wire.endTransmission();
}

// 读取三轴加速度, 单位 g
void readAccel(float &ax, float &ay, float &az) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);   // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, (uint8_t)6);
  int16_t rawX = (Wire.read() << 8) | Wire.read();
  int16_t rawY = (Wire.read() << 8) | Wire.read();
  int16_t rawZ = (Wire.read() << 8) | Wire.read();
  ax = rawX / ACCEL_SCALE;
  ay = rawY / ACCEL_SCALE;
  az = rawZ / ACCEL_SCALE;
}

void loop() {
  float ax, ay, az;
  readAccel(ax, ay, az);

  // 俯仰角: 绕 Y 轴, 用 -ax 与 YZ 平面重力分量
  float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / PI;
  // 横滚角: 绕 X 轴
  float roll  = atan2(ay, az) * 180.0f / PI;

  Serial.print(millis());
  Serial.print(',');
  Serial.print(pitch, 2);
  Serial.print(',');
  Serial.println(roll, 2);

  delay(100);
}

运行与验证

1. 接线：MPU6050 的 SDA→A4、SCL→A5、VCC→3.3V/5V、GND→GND（UNO 内置上拉）。
2. 烧录后打开串口监视器，波特率设为 115200。
3. 观察输出：每行形如 时间,pitch,roll，例如静止水平放置时 pitch≈0、roll≈0。
4. 判定通过：将传感器绕 X 轴倾斜约 90°，roll 应接近 ±90°；绕 Y 轴倾斜约 90°，pitch 应接近 ±90°；数值随姿态连续变化且无跳变。

提示：加速度计对运动加速度敏感，快速晃动时角度会抖动，这是正常现象；本案例只做静态/准静态倾角解算，不涉及陀螺仪融合。