DMP Packet Length Mismatch when using RAW_GYRO (Missing 8-Byte Wrapper) #9
Description
Version
No response
Describe the bug
The mpu6500_dmp_read function incorrectly calculates the packet length when MPU6500_DMP_FEATURE_SEND_RAW_GYRO is enabled (without SEND_CAL_GYRO). The provided DMP firmware blob adds an 8-byte wrapper (4-byte Header 0x20000000 + 4-byte Footer) to Gyroscope packets, but the driver does not account for this overhead.
This results in FIFO misalignment, causing "Error 7: fifo data is too little" and eventual FIFO overflows.
Reproduce
I added some debug prints inside dmp_read:
// Added this:
//handle->debug_print("DEBUG: Calculated len: %d\n", len);
res = a_mpu6500_read(handle, MPU6500_REG_FIFO_COUNTH, (uint8_t *)buf, 2); /* read fifo count */
if (res != 0) /* check result */
{
handle->debug_print("mpu6500: read fifo count failed.\n"); /* read fifo count failed */
return 1; /* return error */
}
count = (uint16_t)(((uint16_t)buf[0] << 8) | buf[1]); /* set count */
// Added this:
//handle->debug_print("DEBUG: FIFO Count: %d\n", count);
did these tests:
Quat + Accel:
Driver Calculated: 22 bytes
Actual FIFO Output: 22 bytes
Result: OK
Quat + Calibrated Gyro:
Driver Calculated: 22 bytes
Actual FIFO Output: 22 bytes
Result: OK (Calibrated Gyro does not have overhead)
Quat + Raw Gyro:
Driver Calculated: 22 bytes
Actual FIFO Output: 30 bytes (Hex dump shows 8-byte wrapper)
Result: FAIL (+8 bytes mismatch)
Expected behavior
Quat + Raw Gyro calculated length should match the actual length and be 30 bytes
Additional context
I don't think this is an issue with my specific implementation or my mpu6500, but changing this in the driver.c inside mpu6500_dmp_read fixed the issue for me.
if ((handle->mask & MPU6500_DMP_FEATURE_SEND_ANY_GYRO) != 0)
{
len += 6;
// FIX: Add 8 bytes overhead ONLY if Raw Gyro is used without Cal Gyro
if ((handle->mask & MPU6500_DMP_FEATURE_SEND_RAW_GYRO) != 0)
{
if ((handle->mask & MPU6500_DMP_FEATURE_SEND_CAL_GYRO) == 0)
{
len += 8;
}
}
}
And changing the actual reading here:
if ((handle->mask & MPU6500_DMP_FEATURE_SEND_ANY_GYRO) != 0)
{
if ((handle->mask & MPU6500_DMP_FEATURE_SEND_RAW_GYRO) != 0)
{
if ((handle->mask & MPU6500_DMP_FEATURE_SEND_CAL_GYRO) == 0)
{
i += 8; // Skip the Header
}
}
}