论坛>航空航天>喷气推进>飞控制导
发表于《求助有关姿态传感器的问题》
tmn_A作者
3年5个月前IP:广东
921480
7楼

再次求助一下各位，我使用chatgpt帮助，再加以改动，直接开了dmp，读取四元数，但是仍然有许多问题，我排查了好几天，都没有找到哪里出了问题。希望能得到帮助。

首先，我怀疑问题可能出现再硬件上，我就先说下我的硬件，用的arduino

Nano，atmega328p。

以下是第一份代码，也是chatgpt在我提了一堆问题后修修补补出来的。以下是代码

#include <Wire.h>
#include "MPU6050.h"
#include "I2Cdev.h"
#include "helper_3dmath.h"

MPU6050 mpu;

Quaternion quat;
VectorFloat gravity;
VectorInt16 aaReal;
VectorInt16 aaWorld;
float ypr[3];

uint8_t mpuIntStatus;
bool dmpReady;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[128];
unsigned long t;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1) {}
  }
  mpu.setDMPEnabled(true);
  mpuIntStatus = mpu.getIntStatus();
  dmpReady = true;
 
  packetSize = mpu.dmpGetFIFOPacketSize();
}

void loop() {
  if (!dmpReady) return;
 
  while (fifoCount < packetSize) {
    fifoCount = mpu.getFIFOCount();
  }
 
  mpu.getFIFOBytes(fifoBuffer, packetSize);
 
  fifoCount -= packetSize;

  mpu.dmpGetQuaternion(&quat, fifoBuffer);
  mpu.dmpGetEuler(ypr, &quat);
  mpu.dmpGetGravity(&gravity, &quat);
  mpu.dmpGetAccel(&aaReal, fifoBuffer);
  mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &quat);
  memset(fifoBuffer, 0, packetSize);// 清空fifoBuffer数组
  if (millis()-t>100){
    Serial.print(" q0: ");
    Serial.print(quat.w);
    Serial.print(" q1: ");
    Serial.print(quat.x);
    Serial.print(" q2: ");
    Serial.print(quat.y);
    Serial.print(" q3: ");
    Serial.print(quat.z);
    Serial.print(" yaw: ");
    Serial.print(ypr[0]*180/M_PI);
    Serial.print(" pitch: ");
    Serial.print(ypr[1]*180/M_PI);
    Serial.print(" roll: ");
    Serial.print(ypr[2]*180/M_PI);
    Serial.print(" gx: ");
    Serial.print((float)gravity.x * 9.8);
    Serial.print(" gy: ");
    Serial.print((float)gravity.y * 9.8);
    Serial.print(" gz: ");
    Serial.print((float)gravity.z * 9.8);
    Serial.print(" arx: ");
    Serial.print(aaReal.x);
    Serial.print(" ary: ");
    Serial.print(aaReal.y);
    Serial.print(" arz: ");
    Serial.print(aaReal.z);
    Serial.print(" awx: ");
    Serial.print(aaWorld.x);
    Serial.print(" awy: ");
    Serial.print(aaWorld.y);
    Serial.print(" awz: ");
    Serial.println(aaWorld.z);    
    t = millis();
  }
}

这个代码的校准值都是chatgpt给的，肯定不对，但是先不管它。这个代码有以下几点问题。有时候，他会输出一段时间正常的，然后突然输出错误的数据，然后错误一段时间后输出正确的数据，然后几分钟后就停止输出了，有时候他会输出正常，但是几分钟后就停止输出了。输出正确的时候数据是没问题的，我手算了下四元数符合，就是校准问题数据在飘而已，还可以接受。这个问题我完全找不到原因。还有，他输出的加速度数据换算来只有0.5g往上

微信截图_20230607121345.png

接下来是第二个代码。我改了量程，并且重新校准，毕竟用在火箭上嘛。这个校准是没问题的，单独输出三轴加速度和角速度的时候非常完美。以下是代码

#include <Wire.h>
#include "MPU6050.h"
#include "I2Cdev.h"
#include "helper_3dmath.h"

MPU6050 mpu;

Quaternion quat;
VectorFloat gravity;
VectorInt16 aaReal;
VectorInt16 aaWorld;
float ypr[3];

uint8_t mpuIntStatus;
bool dmpReady;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[128];
unsigned long t;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  mpu.initialize();
  mpu.dmpInitialize();
  //设置量程
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
  //校准，不同传感器在这里改
  mpu.setXGyroOffset(36);
  mpu.setYGyroOffset(-2);
  mpu.setZGyroOffset(-3);
  mpu.setXAccelOffset(-3252);
  mpu.setYAccelOffset(1713);
  mpu.setZAccelOffset(1507);

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1) {}
  }
  mpu.setDMPEnabled(true);
  mpuIntStatus = mpu.getIntStatus();
  dmpReady = true;
 
  packetSize = mpu.dmpGetFIFOPacketSize();
}

void loop() {
  if (!dmpReady) return;
 
  while (fifoCount < packetSize) {
    fifoCount = mpu.getFIFOCount();
  }
 
  mpu.getFIFOBytes(fifoBuffer, packetSize);
 
  fifoCount -= packetSize;

  mpu.dmpGetQuaternion(&quat, fifoBuffer);
  mpu.dmpGetEuler(ypr, &quat);
  mpu.dmpGetGravity(&gravity, &quat);
  mpu.dmpGetAccel(&aaReal, fifoBuffer);
  mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &quat);
  memset(fifoBuffer, 0, packetSize);// 清空fifoBuffer数组
  if (millis()-t>100){
    Serial.print(" q0: ");
    Serial.print(quat.w);
    Serial.print(" q1: ");
    Serial.print(quat.x);
    Serial.print(" q2: ");
    Serial.print(quat.y);
    Serial.print(" q3: ");
    Serial.print(quat.z);
    Serial.print(" yaw: ");
    Serial.print(ypr[0]*180/M_PI);
    Serial.print(" pitch: ");
    Serial.print(ypr[1]*180/M_PI);
    Serial.print(" roll: ");
    Serial.print(ypr[2]*180/M_PI);
    Serial.print(" gx: ");
    Serial.print((float)gravity.x * 9.8);
    Serial.print(" gy: ");
    Serial.print((float)gravity.y * 9.8);
    Serial.print(" gz: ");
    Serial.print((float)gravity.z * 9.8);
    Serial.print(" arx: ");
    Serial.print(aaReal.x);
    Serial.print(" ary: ");
    Serial.print(aaReal.y);
    Serial.print(" arz: ");
    Serial.print(aaReal.z);
    Serial.print(" awx: ");
    Serial.print(aaWorld.x);
    Serial.print(" awy: ");
    Serial.print(aaWorld.y);
    Serial.print(" awz: ");
    Serial.println(aaWorld.z);    
    t = millis();
  }
}

但是这一次输出仍有代码一的问题，而且更要命的是，正常输出时数据也是错的，比如转90°它会显示到100多，而且四元数都是有问题的。

然后我又改了一遍

#include <Wire.h>
#include "MPU6050.h"
#include "I2Cdev.h"
#include "helper_3dmath.h"

MPU6050 mpu;

Quaternion quat;
VectorFloat gravity;
VectorInt16 aaReal;
VectorInt16 aaWorld;
float ypr[3];

uint8_t mpuIntStatus;
bool dmpReady;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[128];
unsigned long t;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  mpu.initialize();
  mpu.dmpInitialize();
  //设置量程
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
  //校准，不同传感器在这里改
  mpu.setXAccelOffset(-3252);
  mpu.setYAccelOffset(1713);
  mpu.setZAccelOffset(1507);
  /*mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);*/

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1) {}
  }
  mpu.setDMPEnabled(true);
  mpuIntStatus = mpu.getIntStatus();
  dmpReady = true;
 
  packetSize = mpu.dmpGetFIFOPacketSize();
}

void loop() {
  if (!dmpReady) return;
 
  while (fifoCount < packetSize) {
    fifoCount = mpu.getFIFOCount();
  }
 
  mpu.getFIFOBytes(fifoBuffer, packetSize);
 
  fifoCount -= packetSize;

  mpu.dmpGetQuaternion(&quat, fifoBuffer);
  mpu.dmpGetEuler(ypr, &quat);
  mpu.dmpGetGravity(&gravity, &quat);
  mpu.dmpGetAccel(&aaReal, fifoBuffer);
  mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &quat);
  memset(fifoBuffer, 0, packetSize);// 清空fifoBuffer数组
  if (millis()-t>100){
    Serial.print(" q0: ");
    Serial.print(quat.w);
    Serial.print(" q1: ");
    Serial.print(quat.x);
    Serial.print(" q2: ");
    Serial.print(quat.y);
    Serial.print(" q3: ");
    Serial.print(quat.z);
    Serial.print(" yaw: ");
    Serial.print(ypr[0]*180/M_PI);
    Serial.print(" pitch: ");
    Serial.print(ypr[1]*180/M_PI);
    Serial.print(" roll: ");
    Serial.print(ypr[2]*180/M_PI);
    Serial.print(" gx: ");
    Serial.print((float)gravity.x * 9.8);
    Serial.print(" gy: ");
    Serial.print((float)gravity.y * 9.8);
    Serial.print(" gz: ");
    Serial.print((float)gravity.z * 9.8);
    Serial.print(" arx: ");
    Serial.print(aaReal.x);
    Serial.print(" ary: ");
    Serial.print(aaReal.y);
    Serial.print(" arz: ");
    Serial.print(aaReal.z);
    Serial.print(" awx: ");
    Serial.print(aaWorld.x);
    Serial.print(" awy: ");
    Serial.print(aaWorld.y);
    Serial.print(" awz: ");
    Serial.println(aaWorld.z);    
    t = millis();
  }
}

也就是说，只把加速度量程调到16g，角速度还是250°/s。现在就只有代码一的问题了。看样子导致像代码二那样完全没法输出正确的原因还是和角速度有关。

但是，我无法继续排查问题，我真的找不到问题出在哪里。我感觉问题很底层，甚至在硬件上，但这只是感觉。我没有学过单片机，我打算学stm32，但是最近真的很忙没时间，9月才能开始学。希望大家能给点帮助，帮忙看看问题出在哪里。谢谢各位了。

评论
文号 / 921480
tmn_A
百炼成钢
名片专栏发私信
学术分 0
总主题 15 帖总回复 172 楼拥有证书：进士 机友 笔友
注册于 2020/08/28 12:52:17最后登录 2026/10/30 21:53:39
主体类型：个人
所属领域：无
认证方式：手机号
IP归属地：广东

个人简介

辰星引力工作室成员，火箭爱好者，嵌入式爱好者，物理爱好者，还有亚雷斯塔克劳利是我老婆。

笔记
{{note.content}}
{{n.user.username}}
{{fromNow(n.toc)}} {{n.status === noteStatus.disabled ? "已屏蔽" : ""}} {{n.status === noteStatus.unknown ? "正在审核" : ""}} {{n.status === noteStatus.deleted ? '已删除' : ''}}
  • 编辑
  • 删除
  • {{n.status === 'disabled' ? "解除屏蔽" : "屏蔽" }}
我也是有底线的