我有一个项目，我想测量称重传感器的偏航-俯仰-倾角。我正在使用MPU6050测量角度。我读过，要正确校正偏航可能会很困难，因为它会漂移。但是，在我的设置中唯一可响应且正确的角度是偏航。俯仰和横滚都滞后并且非常缓慢地响应方向变化。当我旋转时，它们首先朝相反的方向（由于惯性？）记录运动，然后才向正确的方向爬行。我将Jeff Rowberg的库用作代码中MPU6050的基础。代码附在下面。有谁知道如何固定我的俯仰和滚转？
[在此处输入图片描述] [1]
#include <SD.h>
#include <SPI.h>
File myFile;
#define Red_LED 24
#define Green_LED 25
int CSpin = 8;
float timeStamp = 0;
float timeZero = 0;
int i = 0;
String fileName;
String text;
String number;
// Load Cell x2
#include "HX711.h"
const int DOUT = 5;
const int CLK = 4;
const int DOUT2 = 10;
const int CLK2 = 9;
HX711 scale;
HX711 scale2;
float calibration_factor = -2200; //-19500 worked for my 440lb max scale setup
float offset_factor = 18000; // was -4500
float force = 0;
float force2 = 0;
// Nano comm
#define sync_signal 7
// Gyroscope x2
#include "I2Cdev.h"
#include "Wire.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x69);
MPU6050 mpu2(0x68); // <-- use for AD0 high
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define INTERRUPT_PIN2 3
#define ADDR_PIN 6
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
bool dmpReady2 = false;
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t mpuIntStatus2;
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint8_t devStatus2;
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t packetSize2;
uint16_t fifoCount; // count of all bytes currently in FIFO
uint16_t fifoCount2;
uint8_t fifoBuffer[64]; // FIFO storage buffer, original value: 64 worked for fiforate 0x09 and only getting euler.
uint8_t fifoBuffer2[64];
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
Quaternion q2;
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aa2;
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaReal2;
VectorFloat gravity; // [x, y, z] gravity vector
VectorFloat gravity2;
float ypr[3]; // [psi, theta, phi] Euler angle container
float ypr2[3];
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
volatile bool mpuInterrupt2 = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void dmpDataReady2() {
mpuInterrupt2 = true;
}
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000); //400000
// SD card
pinMode(CSpin, OUTPUT);
if (SD.begin()){
Serial.println("SD ready");
}
else{
Serial.println("SD init failed");
return;
}
// Load Cell
scale.begin(DOUT, CLK);
scale.set_scale(calibration_factor); //Adjust to this calibration factor
//long zero_factor = scale.read_average(); //Get a baseline reading
scale.tare();
Serial.println("Load Cell 1 ready");
scale2.begin(DOUT2, CLK2);
scale2.set_scale(calibration_factor); //Adjust to this calibration factor
//long zero_factor2 = scale2.read_average(); //Get a baseline reading
scale2.tare();
Serial.println("Load Cell 2 ready");
// Nano comm
pinMode(sync_signal, OUTPUT);
pinMode(Red_LED, OUTPUT);
pinMode(Green_LED, OUTPUT);
// Gyroscope x2
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
mpu2.initialize();
pinMode(INTERRUPT_PIN, INPUT);
pinMode(INTERRUPT_PIN2, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(F("Testing device2 connections..."));
Serial.println(mpu2.testConnection() ? F("MPU6050 2 connection successful") : F("MPU6050 2 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
devStatus2 = mpu2.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
mpu2.setXGyroOffset(220);
mpu2.setYGyroOffset(76);
mpu2.setZGyroOffset(-85);
mpu2.setZAccelOffset(1788);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// make sure it worked (returns 0 if so)
if (devStatus2 == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu2.CalibrateAccel(6);
mpu2.CalibrateGyro(6);
mpu2.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu2.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN2));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN2), dmpDataReady2, RISING);
mpuIntStatus2 = mpu2.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady2 = true;
// get expected DMP packet size for later comparison
packetSize2 = mpu2.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus2);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
digitalWrite(Green_LED, HIGH);
}
void loop() {
// put your main code here, to run repeatedly:
if (analogRead(A3) > 1000){ // If button is pushed
Serial.println(F("Measurement starting"));
digitalWrite(Red_LED, HIGH);
digitalWrite(Green_LED, LOW);
i = i + 1;
number = String(i);
fileName = String("ptest" + number + ".txt");
myFile = SD.open(fileName, FILE_WRITE); // Create a new file
if (myFile){
myFile.println("Time Force1 Force2 Y1 P1 R1 Y2 P2 R2");
//myFile.close(); Rapid opening and closing fucked up
}
else{
Serial.println("myFile not found");
}
//Serial.println("Time Force1 Force2 Y1 P1 R1 Y2 P2 R2");
timeZero = millis(); // Set timeZero
int j = 0;
while ( (millis() - timeZero) < 1005000){ // For ?? seconds, take measurements
timeStamp = millis() - timeZero;
/*if (timeStamp > j){
Serial.println(j/1000);
j = j + 1000;
}*/
force = scale.get_units(); // Measure force 1
force2 = scale2.get_units(); // Measure force 2
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Measure angle 1 and accel 1
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
}
if (mpu2.dmpGetCurrentFIFOPacket(fifoBuffer2)) { // Measure angle 2
mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
mpu2.dmpGetGravity(&gravity2, &q2);
mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
mpu2.dmpGetAccel(&aa2, fifoBuffer2);
mpu2.dmpGetLinearAccel(&aaReal2, &aa2, &gravity);
}
digitalWrite(sync_signal, HIGH); // Tell nano to log angle
// calculate angle = euler[0] - euler2[0], angle is defined as float
//myFile = SD.open(fileName, FILE_WRITE); // Create a new file
if (myFile){
/*myFile.print(timeStamp);
myFile.print(",");
myFile.print(force);
myFile.print(",");
myFile.print(force2);
myFile.print(",");*/
Serial.print(- ypr[0] * 180/M_PI);
Serial.print(",");
Serial.print(- ypr[2] * 180/M_PI);
Serial.print(",");
Serial.print(- ypr[2] * 180/M_PI);
Serial.print(" ");
/*myFile.print(aaReal.x);
myFile.print(",");
myFile.print(aaReal.y);
myFile.print(",");
myFile.print(aaReal.z);
myFile.print(",");*/
Serial.print(- ypr2[0] * 180/M_PI);
Serial.print(",");
Serial.print(- ypr2[2] * 180/M_PI);
Serial.print(",");
Serial.println(- ypr2[2] * 180/M_PI);
/*myFile.print(",");
myFile.print(aaReal2.x);
myFile.print(",");
myFile.print(aaReal2.y);
myFile.print(",");
myFile.println(aaReal2.z);*/
}
else{
Serial.println("myFile not found");
}
mpu.resetFIFO();
mpu2.resetFIFO();
digitalWrite(sync_signal, LOW); // Tell nano to stand by
}
myFile.close();
digitalWrite(Red_LED, LOW);
digitalWrite(Green_LED, HIGH);
Serial.println("Measurement ended");
}
}```
[1]: https://i.stack.imgur.com/7qW5m.png
[2]: https://i.stack.imgur.com/Q1N8p.png