已屏蔽 原因：{{ notice.reason }}已屏蔽
屏蔽解除屏蔽编辑详情
{{notice.noticeContent}}
~~空空如也
某倒吊的亚雷斯塔
1年5个月前 IP:广东
 操作
发表于《求助有关姿态传感器的问题》

再次求助一下各位，我使用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
某倒吊的亚雷斯塔
十步芳草
名片发私信
学术分 0
总主题 9 帖总回复 114 楼拥有证书：进士 机友
注册于 2020-08-28 12:52最后登录 2024-10-24 13:00
主体类型：个人
所属领域：无
认证方式：手机号
IP归属地：广东

个人简介

火箭爱好者

文件下载
加载中...
{{errorInfo}}
{{downloadWarning}}
你在 {{downloadTime}} 下载过当前文件。
文件名称：{{resource.defaultFile.name}}
下载次数：{{resource.hits}}
上传用户：{{uploader.username}}
所需积分：{{costScores}}，{{holdScores}}下载当前附件免费{{description}}
下载地址：{{l.name}}
积分不足，去充值
文件已丢失
当前账号的附件下载数量限制如下：
时段 个数
{{f.startingTime}}点 - {{f.endTime}}点 {{f.fileCount}}
视频暂不能访问，请登录试试
登录注册
仅供内部学术交流或培训使用，请先保存到本地。本内容不代表科创观点，未经原作者同意，请勿转载。
预览
音频暂不能访问，请登录试试
登录注册
投诉或举报
加载中...
{{tip}}
请选择违规类型：
{{reason.type}}

空空如也

提交成功！
插入资源
全部
图片
视频
音频
附件
全部
未使用
已使用
正在上传
{{b.num+1}}{{b.num+1}}..
跳转到  确定
空空如也~
上传中..{{f.progress}}%
处理中..
上传失败，点击重试
等待中...
{{f.name}}
空空如也~
(视频){{r.oname}}
{{selectedResourcesId.indexOf(r.rid) + 1}}
处理中..
处理失败
插入表情
我的表情
共享表情
Emoji
上传
{{b.num+1}}{{b.num+1}}..
注意事项
最大尺寸100px，超过会被压缩。为保证效果，建议上传前自行处理。
建议上传自己DIY的表情，严禁上传侵权内容。
点击重试等待上传{{s.progress}}%处理中...已上传，正在处理中
空空如也~
处理中...
处理失败
加载中...
草稿箱
加载中...
此处只插入正文，如果要使用草稿中的其余内容，请点击继续创作。
刷新{{b.num+1}}{{b.num+1}}..
{{fromNow(d.toc)}}
{{getDraftInfo(d)}}
标题：{{d.t}}
内容：{{d.c}}
继续创作
删除插入插入
插入公式
评论控制
加载中...
文号：{{pid}}
加载中...
详情
详情
推送到专栏从专栏移除
设为匿名取消匿名
查看作者
回复
只看作者
加入收藏取消收藏
收藏
取消收藏
折叠回复
置顶取消置顶
评学术分
鼓励
设为精选取消精选
管理提醒
编辑
通过审核
评论控制
退修或删除
历史版本
违规记录
投诉或举报
加入黑名单移除黑名单
查看IP
{{format('YYYY/MM/DD HH:mm:ss', toc)}}
ID: {{user.uid}}
文章
{{user.threadCount - user.disabledThreadsCount}}
回复
{{user.postCount - user.disabledPostsCount}}
学术分
{{user.xsf}}
私信私信
专栏