MPU6050::dmpGetQuaternion<float>() uses only 16 instead of 32 bit resolution

[jonaswgit]

Hey, first of all thanks for this amazing library!!

~~I noticed, that when I use the default implementation from the MPU6050_DMP6.ino example I get about half a degree of resolution for the readable roll pitch yaw angles~~ edit: never mind this, this was for another reason

After digging deeper and seeing if I could further improve this, I noticed the following: (first, here is the important snippet from MPU6050_6Axis_MotionApps20.h)

uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    if (packet == 0) packet = dmpPacketBuffer;
    data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
    data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
    data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
    data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
    return 0;
}
uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    if (packet == 0) packet = dmpPacketBuffer;
    data[0] = ((packet[0] << 8) | packet[1]);
    data[1] = ((packet[4] << 8) | packet[5]);
    data[2] = ((packet[8] << 8) | packet[9]);
    data[3] = ((packet[12] << 8) | packet[13]);
    return 0;
}
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    int16_t qI[4];
    uint8_t status = dmpGetQuaternion(qI, packet);
    if (status == 0) {
        q -> w = (float)qI[0] / 16384.0f;
        q -> x = (float)qI[1] / 16384.0f;
        q -> y = (float)qI[2] / 16384.0f;
        q -> z = (float)qI[3] / 16384.0f;
        return 0;
    }
    return status; // int16 return value, indicates error if this line is reached
}

The Quaternion in the FIFO packet has 32bit of resolution for each of its for components. The implementation of dmpGetQuaternion(int32_t *data,...) uses these full 32 bits. The implementation of dmpGetQuaternion(int16_t *data,...) uses only the 16 most significant bits (which makes sense, since its the int16 implementation). However the dmpGetQuaternion(Quaternion *q,...) (with q being composed of floats) calls the int16 implementation, and therefore also reduces the resolution to 16 bits. As floats are 32bits, wouldn't it make more sense to have this function call the int32t implementation and then devide the values by 2^30(=1073741824) instead of 2^14(=16384)?

[romatou18]

Agree with this, as of today this beautiful Easter monday under lockdown here in NZ, just spent my afternoon comparins this lib with the Sparkfun DMP one to find out that although the Sparkfun lib is short of a few helper notably to get i.e. gravity from the quat, or yaw pitch roll, and the linear acc then it uses 32bits of precision and also features dynamic sensitivy / full range settings for both the gyro and accelerometer:

1. uses Refer to the qToFloat() function: float MPU9250_DMP::qToFloat(long number, unsigned char q) { unsigned long mask = 0; for (int i=0; i<q; i++) { mask |= (1<<i); } return (number >> q) + ((number & mask) / (float) (2<<(q-1))); }

where number is the quaternion item value and q is the definition in bits -2 i.e. 32 - 2 == 30 So for 32bits quat resolution: float MPU9250_DMP::calcQuat(long axis) { return qToFloat(axis, 30); }

2. using dynamic gyro and accelero and mag sensivity / full range / resolution setting

float MPU9250_DMP::calcAccel(int axis) { return (float) axis / (float) _aSense; }

The transition factor given the datasheet of the sensor should be: transition = max_range_setting / sensor_resolution with

• sensor resolution == 32768 meaning so transition

with the max_range_setting i.e. 2G, 4,8 and 16G 2/32768.0 2G => 1/16384.0 4G => 1/8192 8G => 1/4096 16G =>1/2048

/** from inv_mpu.c file from the invensense basic lib

• @brief Get accel sensitivity scale factor.
• @param[out] sens Conversion from hardware units to g's.
• @return 0 if successful. */ int mpu_get_accel_sens(unsigned short *sens) { switch (st.chip_cfg.accel_fsr) { case INV_FSR_2G: sens[0] = 16384; break; case INV_FSR_4G: sens[0] = 8192; break; case INV_FSR_8G: sens[0] = 4096; break; case INV_FSR_16G: sens[0] = 2048; break; default: return -1; } if (st.chip_cfg.accel_half) sens[0] >>= 1; return 0; }

Same for the gyro and MAg:

float MPU9250_DMP::calcGyro(int axis) { return (float) axis / (float) _gSense; }

float MPU9250_DMP::calcMag(int axis) { return (float) axis / (float) _mSense; }

float MPU9250_DMP::calcQuat(long axis) { return qToFloat(axis, 30); }

[romatou18]

@jrowberg I am a very beginner in the IMU world, just started working on my own little project, Now the practical question is : if im correct there are places like in dmpGetQuaternion where the dynamic range setting would need to be added i.e.

Using 32 bits uint32_t

uint8_t MPU6050::dmpGetQuaternion(Quaternion q, const uint8_t packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) int32_t qI[4]; uint8_t status = dmpGetQuaternion(qI, packet); if (status == 0) { q -> w = (float)qI[0] / mpu_get_accel_sens(this->_sensitivy); q -> x = (float)qI[1] / mpu_get_accel_sens(this->_sensitivy); q -> y = (float)qI[2] / mpu_get_accel_sens(this->_sensitivy); q -> z = (float)qI[3] / mpu_get_accel_sens(this->_sensitivy); return 0; } return status; // int16 return value, indicates error if this line is reached }

And now there is something im not sure to understand about the dmpGetGravity() function, when it uses a factor 4096 when extracting linear acceleration.

My understanding is that the input gravity vector is obtained by calling mpGetGravity(int16_t data, const uint8_t packet) which uses the 16384 ( the 2G accelero resolution transition), to transition from DMP FIFO quaternion to 3D vector. I just wanted to check if when extracting the linear acceleration there would be a reason to us the 8G accelero 1/4096 transition ?

1. obtain the gravity 3D vect from raw quat from DMP packet and transition to SI unit 3D accel vector i.e. q[N] = packetValue / 16384 now we have a mG (SI unit accel vector) i.e.

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; }

3. extract the RAW accel data from the FIFO queue, that provides raw value from 0- 32768 mpu.dmpGetAccel(&aa, fifoBuffer);

4. Convert that raw accel XYZ to SI / mG unit i.e. accXSIUnit = rawX / 16384 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);

5. compute the linear acc vector i.e. linearX = accXSIUnit - gravityXSIUnit

uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 v, VectorInt16 vRaw, VectorFloat gravity) { // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) v -> x = vRaw -> x - gravity -> x4096; v -> y = vRaw -> y - gravity -> y4096; v -> z = vRaw -> z - gravity -> z4096; return 0; }

Again im a beginner, I imagine i could be totally wrong, for instance i could be missing the fact that the acceleration data from the FIFO could possibly use a different sensor sensitivity setting by convention ?.

But as this is quite striking to me, and done tests implementing linear acceleration that got me to XYZ 0 accel when no movements supposedly worked ok , using an MPU9250 and the same sensitivity setting both to process the raw acc from FIFO and quat DMP data, then I was wondering if you could have quick look to confirm ?

Cheers !

Romain

[ZHomeSlice]

And now there is something im not sure to understand about the dmpGetGravity() function, when it uses a factor 4096 when extracting linear acceleration.

My understanding is that the input gravity vector is obtained by calling mpGetGravity(int16_t data, const uint8_t packet) which uses the 16384 ( the 2G accelero resolution transition), to transition from DMP FIFO quaternion to 3D vector. I just wanted to check if when extracting the linear acceleration there would be a reason to us the 8G accelero 1/4096 transition ?

I'm a beginner at Quaternion math I just watched a tutorial for the Nth time and still bearly grasp the full usage of it. With the accelerometer, I've beaten it up quite well when I decided to from scratch, redo the MPU6050 library (My Library is Simple_MPU6050 which turns out after sharing what I've learned here, to be similar in final functionality to this one so I haven't pursued it lately). This took months but I'm much more knowledgeable when it comes to the accelerometer and gyro portion of the MPU6050 family.

The rates of acceleration are sent to us as an Int 16 Bit (16384) without a sign. Zero is -2G while 16384 is +2G now zero-g is at 8192 ( note 8192 is always 0g no matter what scale you are using +-1g, +-2g, +-4g, etc.)

+-2g has a full range of 4g with 8192 equalling zero-g +8192 and -8192 16384/4 = 4096 or 1g

Hope this helps Z

[romatou18]

Hi,

Thanks a lot for your answer ! this helps ! Cheers