Hi all,
I'm building a quadcopter with the use of the MPU 6050.
The dmp6 example sketch from Jeff Rowberg is used to make use of the DMP.
I've set the update frequency to 100 Hz, and it gives stable, smooth signals.
However, the filtering time seems a bit long and I am wondering if this can be shortened?
Attached is a example where i tilt the sensor and put it back on my desk, it takes about 5 seconds for the signal to stabilize.
does anyone know if this is possible and how?
Thanks,
The sketch from Jeff Rowberg is used
but not posted. I'm leaving it to you to guess what sketch I'm talking about.
Did I paraphrase your post correctly?
Added link to the sketch
It's somewhere around line 300 (sorry using my cell to help you) look for this here MPU6050_6Axis_MotionApps20.h
0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
// It is important to make sure the host processor can keep up with reading and processing
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
};
;
Hi zhomeslice, thanks for the reply
I think the line you mean changes the sample rate.
Varying between 20 and 200 hz. I've set this to 100 hz, but does not seem to have much influence on the time it takes for the sensor to stabilise. The sample shown in the post is at 100 hz, as fast as it will reliably go.
KingBee:
Hi zhomeslice, thanks for the replyI think the line you mean changes the sample rate.
Varying between 20 and 200 hz. I've set this to 100 hz, but does not seem to have much influence on the time it takes for the sensor to stabilise. The sample shown in the post is at 100 hz, as fast as it will reliably go.
I have tried all the frequencies 20-200 hz and the 100 works well for the UNO. I can calculate multiple PID loops and control motors and have time to spare.
Now I see your comment "sensor to stabilize" My Balancing bot always skips about 100 cycles before it will consider using the data but it is reliable after that. Yaw is another story and it drifts a bit longer but usually 2-3 seconds and it stabilizes. I bought cheap MPU6050's and I guess they may have some issues. So if your MPU-6050 drifts for longer periods or never seems to lock in. have you tried the calibration programs to set the offsets?
Calibration code MPU6050_calibration.ino
Code I use to initialize my MPU6050 for FIFO:
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
Place this up top for easy access
// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino
// -4232 -706 1729 173 -94 37
// XA YA ZA XG YG ZG
int MPUOffsets[6] = { -4232, -706, 1729, 173, -94, 37};
//Other code
void MPU6050Connect() {
static int MPUInitCntr = 0;
// initialize device
mpu.initialize(); // same
// load and configure the DMP
devStatus = mpu.dmpInitialize();// same
if (devStatus != 0) {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
MPUInitCntr++;
Serial.print(F("MPU connection Try #"));
Serial.println(MPUInitCntr);
Serial.print(F("DMP Initialization failed (code "));
Serial.print(StatStr[devStatus]);
Serial.println(F(")"));
if (MPUInitCntr >= 10) return; //only try 10 times
delay(1000);
MPU6050Connect(); // Lets try again
return;
}
mpu.setXAccelOffset(MPUOffsets[0]);
mpu.setYAccelOffset(MPUOffsets[1]);
mpu.setZAccelOffset(MPUOffsets[2]);
mpu.setXGyroOffset(MPUOffsets[3]);
mpu.setYGyroOffset(MPUOffsets[4]);
mpu.setZGyroOffset(MPUOffsets[5]);
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
Serial.print("mpu.getInterruptDrive= ");Serial.println(mpu.getInterruptDrive());
attachInterrupt(0, dmpDataReady, RISING); //pin 2 on the Uno
mpuIntStatus = mpu.getIntStatus(); // Same
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
delay(1000); // Let it Stabalize
mpu.resetFIFO(); // Clear fifo buffer
mpu.getIntStatus();
mpuInterrupt = false; // wait for next interrupt
}
Just tried a new calibration, results are quite similar to yours.
Sensor takes about 10 seconds to stabilize and then produces a steady signal. After a big movement (in any direction) still requires a significant amount of time to recover to its original position.
When I look at the raw data it seems to be correct, and no delay is present.
So my thoughts are that there is some sort of long averaging time activated somewhere
any ideas?
KingBee:
Just tried a new calibration, results are quite similar to yours.
Sensor takes about 10 seconds to stabilize and then produces a steady signal. After a big movement (in any direction) still requires a significant amount of time to recover to its original position.When I look at the raw data it seems to be correct, and no delay is present.
So my thoughts are that there is some sort of long averaging time activated somewhereany ideas?
I have an Idea... and my code
so the FIFO buffer can contain about 24 reading which if you were reading the first one and it could be 240 MS old before we get buffer overflow. How are you gathering your readings?
This is the DMP FIFO retrieval code I am using on my balancing bot VIDEO
(Extra Notes added with lots of Failure testing output this can be simplified)
void GetDMP(bool Startup) { // Best version I have made so far
mpuInterrupt = false;
fifoCount = mpu.getFIFOCount();
/*
fifoCount is a 16-bit unsigned value. Indicates the number of bytes stored in the FIFO buffer.
This number is in turn the number of bytes that can be read from the FIFO buffer and it is
directly proportional to the number of samples available given the set of sensor data bound
to be stored in the FIFO
*/
// PacketSize = 42; refference in MPU6050_6Axis_MotionApps20.h Line 527
// FIFO Buffer Size = 1024;
uint16_t MaxPackets = 20;// 20*42=840 leaving us with 2 Packets (out of a total of 24 packets) left before we overflow.
// If we overflow the entire FIFO buffer will be corrupt and we must discard it!
// At this point in the code FIFO Packets should be at 1 99% of the time if not we need to look to see where we are skipping samples.
if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (fifoCount < packetSize)) { // we have failed Reset and wait till next time!
digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
Serial.println("Reset FIFO");
if (fifoCount % packetSize) Serial.print("\t Packet corruption"); // fifoCount / packetSize returns a remainder... Not good! This should never happen if all is well.
Serial.print("\tfifoCount "); Serial.print(fifoCount);
Serial.print("\tpacketSize "); Serial.print(packetSize);
mpuIntStatus = mpu.getIntStatus(); // reads MPU6050_RA_INT_STATUS 0x3A
Serial.print("\tMPU Int Status "); Serial.print(mpuIntStatus , BIN);
// MPU6050_RA_INT_STATUS 0x3A
//
// Bit7, Bit6, Bit5, Bit4 , Bit3 , Bit2, Bit1, Bit0
// ----, ----, ----, FIFO_OFLOW_INT, I2C_MST_INT, ----, ----, DATA_RDY_INT
/*
Bit4 FIFO_OFLOW_INT: This bit automatically sets to 1 when a FIFO buffer overflow interrupt has been generated.
Bit3 I2C_MST_INT: This bit automatically sets to 1 when an I2C Master interrupt has been generated. For a list of I2C Master interrupts, please refer to Register 54.
Bit1 DATA_RDY_INT This bit automatically sets to 1 when a Data Ready interrupt is generated.
*/
if (mpuIntStatus & B10000) { //FIFO_OFLOW_INT
Serial.print("\tFIFO buffer overflow interrupt ");
}
if (mpuIntStatus & B1000) { //I2C_MST_INT
Serial.print("\tSlave I2c Device Status Int ");
}
if (mpuIntStatus & B1) { //DATA_RDY_INT
Serial.print("\tData Ready interrupt ");
}
Serial.println();
//I2C_MST_STATUS
//PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
mpu.resetFIFO();// clear the buffer and start over
mpu.getIntStatus(); // make sure status is cleared we will read it again.
} else {
while (fifoCount >= packetSize) { // Get the packets until we have the latest!
if (fifoCount < packetSize) break; // Something is left over and we don't want it!!!
mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
fifoCount -= packetSize;
}
MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
if (fifoCount > 0) mpu.resetFIFO(); // clean up any leftovers Should never happen! but lets start fresh if we need to. this should never happen.
}
}
void MPUMath(){
// we just got our DMP data Lets use it before it gets old :)
}
i've also bought some MPU6050 on the cheap.
Have 2 remaining both have a lag, so I'm doubtful if the cause is due to a faulty sensor.
I'm using the standard DMP6 example for Jeff's library (see below).
and the lag extends well beyond the 240 ms, talking more in the range of 5 seconds.
did you sensor display this lag as well on the example sketch?
void loop ()
if (!dmpReady) return;
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180 / M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180 / M_PI);
}
Solved....
apparently the settings of the gyro and accelerate range were not set properly.
After adding them to the code, and setting both to 3 everything works fine .. :o :o :o