Kauai Labs releases port of latest Invensense MPU-6050/9150 driver to Arduino Platform

21 Replies to “Kauai Labs releases port of latest Invensense MPU-6050/9150 driver to Arduino Platform”

1. Rather tech question. Have you tried to change full scale gyro range to anything than 2000? I tried like 5 different libs. All of them gives incorrect response like faster rotation of quaternions. 10 degrees of board rotation produces like 360 degrees of rotation in quaternions (checked through famous teapot demo). There is no answer on invensense forums or arduino forums. Google cant find strings other than “mpu_set_gyro_fsr(2000)” so I suppose nobody ever tried to change gyro fsr?

   1. The Invensense documentation in the latest (version 6.1) driver indicates that the DMP only works at a setting of 2000dps and 2G. It’s actually a comment in the example code (mllitest) in the 6.1 SDK. In order to get sensor fusion working at different Full scale range settings, you need to use the software library in the motion driver 6.1 package – and the released versions are binaries that only work on the ARM Cortex processors and the TI MSP processor.

2. Hi,
   Looking at the code, your are using interrupt on magnetometer data, but not with MPU FIFO like on Jeff Rowberg code.
   Can you explain me why?
   Thanks!

   1. Both magnetometer and MPU are interrupt driven. The mpu interrupts are handled within the invensense drivers. It’s configured in the initialize_mpu function in nav6.ino, it specifies the PIN number the interrupt occurs on.

3. Hi!
   I’ve been fighting with random glitches in the communication between two MPU6050 (sharing the same bus, different address) and the Arduino.
   I’m polling both Fifo’s by checking the “data ready” register, and I can get nice quaternion data for some time, but once in a while there are some glitches in the communication, meaning a non-sense quaternion once in a while.
   Can the quaternion data be pulled out from a DMP register instead of the FIFO? If so, which register?
   Thanks!
   Regards!

   1. The preferred design is to wait for in interrupt from the device before reading the FIFO. This avoids unnecessary bus traffic. Another trick in the invensense code is to range check the quaternion values. The valid range is -2 to 2. You can find some range checking code in the invensense drivers. One other thing I’ve seen is if a i2c bus error occurs, and a partial read occurs, therefore subsequent reads are out of synch until the FIFO is drained.

4. Thank you for your reply!
   Yes, I’m waiting for the logic interrupt (by checking the register) and I also check when the FIFO has the expected packet size, 14 bytes only, faster read! Also, I’m draining (reseting) each FIFO every read. When reading one sensor it’s 100% flawless, but with two in the same bus it gets corrupted.
   Thank you for pointing out the checking valid quaternion range code, I have missed it (quite complex for me to understand)! Apart from that, do you think that there is a way to “checksum”? The big issue with that is that it will add latency, which in my application is a problem!
   I might have to go “Raw+Complementary Filter” which is a shame because the DMP is soooooo good!!
   Again, thank you!

   1. Is the I2c read from the FIFO being initiated directly within the interrupt service routine? If so, it’s possible the second mpu interrupt is interrupting the first i2c bus transfer, which will abort the i2c transaction initiated by the first. I’m suspicious the errors when two are present are due to i2c bus errors.

      The official invensense motion driver has the quaternion range check, which might help. But there is no foolproof method (like a checksum) on the transaction to verify data integrity – the best approach is to get to the root cause of the transfer errors.

5. BTW, I’m using Jeff Rowberg MPU6050 and DMP libraries, do you think it’s better to use the official driver you ported?

6. Hi,
   Thanks for your answer on the MPU interrupt. I have new questions 🙂
   – MPU calibration : it is performed within your “run_mpu_self_test” function. This one is not activated in nav6 setup. Is there a reason for that? Invensense motion driver tutorial suggests using self test function then mpu_set_accel_bias for accelerometer calibration, and the automatic calibration for the gyroscope. Instead, you use the self test then dmp_set_accel_bias and dmp_set_gyro_bias.
   – The nav6 board has the MPU FSYNC pin connected. I am not using it on my sketch. What is the purpose of this pin and is it used in the nav6?

   Thanks

   1. – MPU calibration. In the nav6 w/the MPU-6050, multiple calibration approaches were attempted, and the one that’s there now ended up working the best based on the testing. In the navX MXP, w/the MPU-9250 the process is to have a “one-time” calibration step which runs the self test and then waits until the DMP calibration has completed (by monitoring some registers, this is not documented), and then reading the DMP gyro bias and storing that to flash memory. Then next time it starts it reloads the gyro bias to the dmp. However, after lots of working w/the accel bias, it was found that the accel biases from the self test were not reliable enough to be stored to flash and used later. So anyways, I’d recommend trying each approach and finding what works best for you.

      – FSYNC: This stands for “Frame Synch” and is designed to be used for optical image stabilization. This is used to synchronize the timing of when the image is acquired and when the corresponding gyroscope data is acquired. The closer they can be aligned in time, the more accurate the optical image stabilization can be. This pin was connected on the nav6 board, but was never actually used.

7. Hi
   I am now trying to timestamp my YPR data. I am using the dmp_read_fifo() function for that. It seems to me that the timestamp in here is the time at which the data is read in the FIFO. I would like the time at which data is generated. In addition, the samples are not equally 10ms spaced.
   YPR 2.07 -61.80 -1.12 TimeStamp 328656
   YPR 2.07 -61.80 -1.12 TimeStamp 328664
   YPR 2.07 -61.80 -1.12 TimeStamp 328672
   YPR 2.07 -61.80 -1.12 TimeStamp 328679
   YPR 2.07 -61.80 -1.12 TimeStamp 328686
   YPR 2.07 -61.80 -1.12 TimeStamp 328695
   YPR 2.07 -61.80 -1.12 TimeStamp 328706
   YPR 2.07 -61.80 -1.12 TimeStamp 328715
   YPR 2.07 -61.80 -1.12 TimeStamp 328725
   YPR 2.07 -61.80 -1.12 TimeStamp 328735
   YPR 2.07 -61.80 -1.12 TimeStamp 328745
   YPR 2.07 -61.80 -1.12 TimeStamp 328756
   YPR 2.07 -61.80 -1.12 TimeStamp 328765

   Another way would be to store the time at which the iterrupt occured, using the millis() function in the gyro_data_ready_cb interrupt function. But I am not very sure of it.
   What would you suggest?

8. The timestamp is acquired from the Arduino millis() function, and the timestamp is acquired each time the loop runs when the interrupt for the MPU-6050 has been generated. If you needed greater accuracy, you could move the acquisition of the timestamp into the gyro_data_ready_cb interrupt service routine, as you say. It may be that the reason why the timestamp is not exactly 10ms greater each time is that sometimes the main loop is processing the serial port or performing calculations, which case acquiring the timestamp in the gyro_data_ready_cb should address that. The general rule for interrupt service routines is to keep them as short as possible, and I’d recommend you follow that guideline in this case.

   1. OK, I had a printout to an LCD which caused the uneaven spacing of data. If I acquire the timestamp in the gyro_data_ready_cb, I get a timestamp 1ms higher than with the first methode and I could figure out the reason why yet. Any idea?

      Thaks!

      YPR -0.32 -59.27 -1.15 TimeStamp 32886 Interruption Timestamp 32887
      YPR -0.32 -59.27 -1.15 TimeStamp 32897 Interruption Timestamp 32898
      YPR -0.32 -59.27 -1.15 TimeStamp 32906 Interruption Timestamp 32907
      YPR -0.32 -59.27 -1.15 TimeStamp 32916 Interruption Timestamp 32917
      YPR -0.32 -59.27 -1.15 TimeStamp 32926 Interruption Timestamp 32927
      YPR -0.32 -59.27 -1.15 TimeStamp 32936 Interruption Timestamp 32937

9. Hi Thiba,

   Actually these timestamps look very good to me. Only one of them is off by 1ms, and this actually pretty normal behavior. The Arduino millis() function is incremented during the “system timer” interrupt service routine. So if anything “holds off” that interrupt function for a very short period of time (that’s entirely possible, for instance if the MPU-6050 interrupt fires off right before the Arduino system timer interrupt does), the millisecond counter will be “late”.

   So in summary, I don’t see any problem w/the timestamps. However, it appears the yaw, pitch and roll values haven’t changed at all in each of the updates you posted, which is possible but usually it changes by a little bit so you might want to double-check that.

   1. Thanks for the answer.
      Yes they look pretty good actually. But I still can figure out why the timestamp made inside the ISR gives 1ms more thant the one in dmp_read_fifo(). Anyway the accuracy is good enough.
      The next step of my datalogger project is to accurately timestamp my GPS measurement. I want ultimately to apply a fusion algorithm on the GPS+MPU measurement, in order to get more accuracy on GPS positionning.
      I want also to output this on an LCD, but this takes long process time and seems to mess up timestamp.

10. Can you post your code? That might make it easier to help determine what’s causing time stamp delay.

11. For this test I used nav6 code with minor addition :

    volatile unsigned long IntTimestamp = 0;

    void loop() {

    if (hal.new_gyro && hal.dmp_on) {

    ……

    Serial.print(“YPR “);
    Serial.print(x);
    Serial.print(” “);
    Serial.print(y);
    Serial.print(” “);
    Serial.print(z);
    Serial.print(“\t”);
    Serial.print(“TimeStamp : “);
    Serial.print(sensor_timestamp);
    Serial.print(“\t”);
    Serial.print(“Interruption Timestamp : “);
    Serial.println(IntTimestamp);
    }

    void gyro_data_ready_cb(void) {
    IntTimestamp = millis();
    hal.new_gyro = 1;
    }

    I see that the serial baud rate makes a difference :

    57600 kbps
    YPR -13.12 -56.91 -1.77 TimeStamp : 6117 Interruption Timestamp : 6119
    YPR -13.17 -56.94 -1.76 TimeStamp : 6129 Interruption Timestamp : 6134
    YPR -13.20 -56.95 -1.76 TimeStamp : 6141 Interruption Timestamp : 6145
    YPR -13.24 -56.98 -1.76 TimeStamp : 6153 Interruption Timestamp : 6159
    YPR -13.28 -57.00 -1.76 TimeStamp : 6165 Interruption Timestamp : 6169
    YPR -13.31 -57.02 -1.76 TimeStamp : 6177 Interruption Timestamp : 6184
    YPR -13.36 -57.04 -1.76 TimeStamp : 6190 Interruption Timestamp : 6194
    YPR -13.40 -57.06 -1.76 TimeStamp : 6202 Interruption Timestamp : 6204
    YPR -13.43 -57.08 -1.76 TimeStamp : 6214 Interruption Timestamp : 6219

    115200 kbps
    PR -0.02 -2.80 0.65 TimeStamp : 1667 Interruption Timestamp : 1668
    YPR -0.02 -3.16 0.62 TimeStamp : 1677 Interruption Timestamp : 1678
    YPR -0.02 -3.52 0.60 TimeStamp : 1687 Interruption Timestamp : 1688
    YPR -0.03 -3.87 0.57 TimeStamp : 1697 Interruption Timestamp : 1698
    YPR -0.04 -4.22 0.55 TimeStamp : 1707 Interruption Timestamp : 1708
    YPR -0.04 -4.57 0.52 TimeStamp : 1717 Interruption Timestamp : 1718
    YPR -0.04 -4.92 0.49 TimeStamp : 1727 Interruption Timestamp : 1728
    YPR -0.05 -5.27 0.46 TimeStamp : 1737 Interruption Timestamp : 1738

12. That’s very helpful. This implies that the priority of the interrupt from the MPU-6050 may be held off by the serial “transfer complete” interrupt which occurs typically on each character. So a faster baud rate (or a fewer amount of characters printed out) will impact the performance, as you have shown. The MPU-6050 interrupt is fed into the INT0 pin on the nav6. It may be that the Arduino libraries are disabling interrupts somewhat, or that the Arduino interrupt handler for the serial communications is taking a long time.

    1. Yes, I need to use a baud rate of at least 115200bps to avoid FIFO overflow. But still, this does not explain why the interrupt timestamp has always 1ms more than the one collected in dmp_read_fifo(), since this one is supposed to take place after the interrupt.

13. Everything is very open with a precise explanation of
    the issues. It was definitely informative. Your site is useful.

    Thanks for sharing!

Comments are closed.