Bugs in RP2040/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h

[stepkun]

Thanks for your great work!

Using the MPU6050 with a Pi Pico I had some issues in MPU6050_6Axis_Motionpps_V6_12.h:

In line 52 is a semicolon missing at the end (strangely this only matters sometimes)

The function dmpGetLinearAccel starting in line 437 seems to use the wrong value for 1g, I think with 2g resolution it should use 16384 not 8192 in lines 439 to 441 (at least with my chip the function than works correct).

Kind regards Stephan

[ZHomeSlice]

+- 2g @ 16384 steps per G 8192 steps per G looks to be a +-4g value

// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
    // get rid of the gravity component (+1g = +16384 in standard DMP FIFO packet, sensitivity is +-2g)
    v -> x = vRaw -> x - gravity -> x*16384 ; // was 8192
    v -> y = vRaw -> y - gravity -> y*16384 ; // was 8192
    v -> z = vRaw -> z - gravity -> z*16384 ; // was 8192
    return 0;
}

does making this change correct values. I was not involved in creating this function but I believe you are correct.

Z

[stepkun]

Yes, that worked for me. I found the same issue in another function. In line 457 it is also calculated with 8192:

uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
   /* +1g corresponds to +8192, sensitivity is 2g. */
   int16_t qI[4];
   uint8_t status = dmpGetQuaternion(qI, packet);
   data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
   data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
   data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
   return status;
}

compared with function in line 468

uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
   v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
   v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
   v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
   return 0;
}

I think it should be

uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
   /* +1g corresponds to +16384, sensitivity is 2g. */
   int16_t qI[4];
   uint8_t status = dmpGetQuaternion(qI, packet);
   data[0] = 2 * ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
   data[1] = 2 * ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
   data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / 16384;
   return status;
}

[Hortay]

Excuse me, could you help me? With "mpu6050_6axis_motionapps20.h" serial freezes, but with "MPU6050_6Axis_MotionApps612.h" the code is not stuck. I have read the files, but I have not found the reasons for the error. I can't use 612, because world accels are not correct. Fastwire, 400kHz, atmega328p, 5 v

[ZHomeSlice]

MPU6050 Freezing is usually caused by the i2c timeout not being set

  Wire.begin();
  Wire.setClock(400000);

This was added to combat the issue. Z

[Hortay]

I have it all installed Can I show my code?

[Hortay]

i found that after a while mpu flag becomes 1 and mpu.dmpGetCurrentFIFOPacket (fifoBuffer) condition is not met

[ZHomeSlice]

Please show your code.

[Hortay]

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu; uint8_t fifoBuffer[64]; volatile bool mpuflag = false; Quaternion q; VectorInt16 aa; VectorInt16 aaReal; VectorInt16 aaWorld; VectorFloat gravity; float ypr[3]; void dmpDataReady(){ mpuflag = true;}

float v0 = 0; float z0 = 0; int col = 0; int tim1;

void setup() { Fastwire::setup(400, true); Serial.begin(115200); mpu.initialize(); mpu.dmpInitialize();

mpu.setXAccelOffset(-538); mpu.setYAccelOffset(1763); mpu.setZAccelOffset(332); mpu.setXGyroOffset(107); mpu.setYGyroOffset(-59); mpu.setZGyroOffset(69);

// mpu.CalibrateAccel(6); // mpu.CalibrateGyro(6); // mpu.PrintActiveOffsets();

mpu.setDMPEnabled(true); attachInterrupt(0, dmpDataReady, RISING); }

void loop() { static int alia; if (mpuflag && mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // Serial.print(degrees(ypr[0])); // вокруг оси Z // Serial.print(','); // Serial.print(degrees(ypr[1])); // вокруг оси Y // Serial.print(','); // Serial.print(degrees(ypr[2])); // вокруг оси x // Serial.println(); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); mpuflag = false;

// Serial.print("aworld\t"); // Serial.print(float(aaWorld.x) / 8192); // Serial.print("\t"); // Serial.print(float(aaWorld.y) / 8129); // Serial.print("\t"); // Serial.print(float(aaWorld.z) / 8192); float osa = float(aaWorld.z) / 8192; // Serial.println(); tim1 = millis(); if(tim1 > 3000){

if(abs(int(osa * 1000)) > 15){
  col = 0;
  v0 = v0 + osa * (tim1 - alia) * 0.001;
  z0 = z0 + v0 * (tim1 - alia);                 
  }
else{
  col += 1;
  if(col >= 30)
    v0 = 0;
  }
  
alia = tim1;
Serial.println(z0);

}} }

[Hortay]

with 612 i have not freezes in mpu i chose Fastwire

[ZHomeSlice]

note that the interrupt has other uses than just FIFO packet arriving in every attempt I have messed with it tends to send 2 interrupts for every packet sent one interrupt is fake and the second is real.

try:

if (mpuflag}{
mpuflag = false;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
// your code here

  }
}

[stepkun]

I sometimes have freezes with 612 which I can "solve" by completely unplug the device from any power supply, wait a few seconds and then do a cold start. Not yet sure what the reason is, it seems to be a problem when (re)uploading the dmp program code into an already initialized and loaded device.

[Hortay]

Sorry for not describing the problem correctly. I am using a library to calculate accelerations. In version 612, the world z-acceleration has a value that includes gravity (1 g), so I'm trying to set up version 20 where accelerations are calculated correctly. But there is a problem with dmpGetCurrentFIFOPacket which stops the code for 30 seconds. I want to customize the library for myself so that it works without errors, but at the moment I need advice on where to start, since now I am still a beginner and understand little. No settings with timer delays help. Friezes also appeared from 612, but they disappeared when fastwire was used.

[ZHomeSlice]

2.0 code has no documentation the only reason it is still around is for the size of the embedded DMP binary. consider correcting the functions for gravity in the 6.12 DMP version I believe this was discussed in a post just prior to yours.

I am not an expert on quaternion math so I am letting someone else proof the corrections.

Z

[techie66]

Just came across this bug as I was looking to file the same bug. I concur, changing 8192 in the mentioned functions to 16384 results in correct numbers. The way they are now only removes half of the gravity. I'd be happy to make the changes and PR it.