Incorrect Gyro and Quaternion data from DMP FIFO.
Closed this issue · 8 comments
I want to debug Navx-MXP via ST-link v2. I took your project for STM32 and followed instructions in the INVENSENSE_LICENSE_README.txt file (but I installed Motion Driver v6.12 instead of v6.1). So when I load the firmware Magnetometer and Accelerometer data seem to be Ok, but something is wrong with Gyro and Quat data, so YPR are wrong too:
What can be the reason of this? This wrong data comes from FIFO buffer
It's recommended to review the navX-Sensor Register Data Types section on this advanced reference page.
I checked it, but I don't understand how can it help me with wrong raw data. Because as you can see gyto_raw[2] has normal values, even when they all were read in little-endian format.
I don't know what normal values means. However, the advanced reference page indicates that the raw data is in device units. Device Units are documented in the MPU-9250 datasheet, and are 16-bit signed values (-32768 to 32768) that represent the Full Scale Range (FSR), which for navX-MXP is +/- 2000 degrees per second (DPS). Using this scale, you can convert these instantaneous gyro rotation values to DPS. But to actually use these values to calculate angle, the instantaneous gyro rotation values need to be integrated over time, and then fused with the accelerometer data to yield YPR/Quaternion data (which happens in the onboard MPU-9250 motion processor). I'm not aware that anyone uses raw gyro, preferring instead the higher-level values of YPR and Quaternion.
Thanks for the reply! "I don't know what normal values means." - I meant that gyto_raw[2] returns values similar to the actual raw values of the gyroscope, but the rest gyto_raw[0] and gyto_raw[1] always return values like in the screenshot (constantly around -16300). And also about Quaternion data - this data is also "incorrect", because w, x, y, z values change their signs every iteration like that:
-17532
21034
17982
154
// the next iteration could be be:
16983
-22093
-17985
190
I think it's innormal quaternion values, because the yaw value change too fast, even when the controller is not moving.
The navX Register Data Types reference is for the navX-MXP registers (YPR, Quaternions, Gyro/Accel Raw Data). The Raw Data format in the navX-MXP registers is the same as the MPU-9250 DMP FIFO, but the quaternion data out of the DMP FIFO is encoded in the Q16.16 format. If you look at the mpucontroller.cpp code, in the get_dmp_data() function, you'll see this code:
Quaternion q_in( (((float)(pdata->quaternion[0]))/65536) / 16384.0f,
(((float)(pdata->quaternion[1]))/65536) / 16384.0f,
(((float)(pdata->quaternion[2]))/65536) / 16384.0f,
(((float)(pdata->quaternion[3]))/65536) / 16384.0f);
That's what converts DMP FIFO formatted quaternions into units used by the Quaternion class, which ultimately gets converted later into navX-sensor register formatted quaternions documented in the navX-sensor Register Data Types.
The MPU9250 DMP FIFO and it's operation are not well documented, so you're likely going to need to dig into the source code including the Invensense source code if you're going to be working with the DMP FIFO.
At a minimum, you're going to need to become intimate with the code in mpucontroller, especially get_dmp_data(), if you want to work directly with the DMP FIFO.
**
The other thing to note is that when the MPU-9250 is in "startup calibration" (basically, the first 30 seconds or so of operation) the values are going to be changing a lot. Unlike more modern sensors that can calibrate more quickly, this older technology takes awhile to stabilize. So this factor may also be contributing to rapidly changing values esp during startup.
Thanks for your reply!
That's what converts DMP FIFO formatted quaternions into units used by the Quaternion class, which ultimately gets converted later into navX-sensor register formatted quaternions documented in the navX-sensor Register Data Types.
Yes, I undestood that, but this code does not change the quaternion values themselves, but only converts them to another data type.
The MPU9250 DMP FIFO and it's operation are not well documented, so you're likely going to need to dig into the source code including the Invensense source code if you're going to be working with the DMP FIFO.
I checked INVENSENSE's files inv_mpu_dmp_motion_driver.c and inv_mpu.c. Saw dmp_read_fifo(...) and mpu_read_fifo_stream(...) functions there. but they seem to be ok and all registers seem to be for MPU9250 or MPU6500. But anyway there could be an issue in their code, so I asked almost the same question in INVENSENSE forum , but I think there won't be any replies.
The other thing to note is that when the MPU-9250 is in "startup calibration" (basically, the first 30 seconds or so of operation) the values are going to be changing a lot.
I have waited for 2 minutes after uploading firmware, but Quaternion data were changing as I said before.
I don't know what to do, may be I should check each register in MPU initializations to be sure, that DMP is configured normally?
I doubt you'll get a response on the Invensense forum, they never replied to us 7 years ago when we developed the product, and we had to figure it out ourselves by trial and error.
That said, the code that is checked in is what was used on the navX-MXP/navX-Micro products, which has been used on thousand of robots including several FIRST FRC championship robots. So I'm confident that this code works and generates decent quaternions; the Yaw, Pitch and Roll angles used in those products are calculated from the Quaternions using the code in the master branch of the navxmxp Github. So my best recommendation is to do as you say - review any changes that might have been made, and any assumptions you may be making. The quaternion values are generated directly by the MPU-9250, so the values will be impacted by all of the configuration register settings.
Some of the values loaded into the MPU-9250 configuration registers are "factory calibration" values that are stored in the STM32 flash memory, so it's recommended to review this too. For instance, if "factory calibration" previously stored calibration data taken when the sensor was in "Z-Up" orientation, but now the sensor is in a different axial orientation, the "factory calibration" should be re-run.
Ok, thanks for all your replies! I will check all configuration registers.