Improved MPU-9150 driver performance

README.md

@@ -14,19 +14,19 @@ Check out www.richards-tech.com for more details, updates and news.
 
 # Release history
 
-## Current
+## April 7 2014 - 0.9.1
 
-### Partial support for STM L3GD20H/LSM303D IMUs
+### Improved performance with MPU-9150
 
-This is in a very early state and only supports the gyro sensor at the moment.
+A new caching strategy for the MPU-9150 seems to be achieving 1000 samples per second without fifo overflows using a 900MHz Raspberry Pi and 400kHz I2C bus. This is as reported by RTIMULibDrive with a CPU utilization of 28%. RTIMULibDemo manages 890 samples per second with the MPU-9150 set to 1000 samples per second. The driver gracefully handles this situation although there is increased delay when the application cannot handle the full sample rate.
 
-### Improved performance with MPU9150
+### Auto detection of IMU
 
-Now can achieve rates up to around 860 Kalman-fused samples per second using the MPU9150 and 900MHz Raspberry Pi.
+RTIMULib can now scan for supported IMUs and configure automatically. This is the default behavior now. It handles IMUs at alternate address automatically as well (for example, it will detect an MPU-9150 at 0x68 or 0x69).
 
-### Auto detection of IMU
+### Partial support for STM L3GD20H/LSM303D IMUs
 
-RTIMULib can now scan for supported IMUs and configure automatically. This is the default behavior now.
+This is in a very early state and only supports the gyro sensor at the moment.
 
 ## April 4 2014 - 0.9.0
 

RTIMULib/RTIMUMPU9150.cpp

@@ -141,6 +141,10 @@ bool RTIMUMPU9150::IMUInit()
 
     m_firstTime = true;
 
+#ifdef MPU9150_CACHE_MODE
+    m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+
     // set validity flags
 
     m_imuData.fusionPoseValid = false;
@@ -422,28 +426,93 @@ bool RTIMUMPU9150::IMURead()
     if (count == 1024) {
         HAL_INFO("MPU9150 fifo has overflowed");
         resetFifo();
+        m_imuData.timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
         return false;
     }
 
-    if (count > 480) {
+
+#ifdef MPU9150_CACHE_MODE
+    if ((m_cacheCount == 0) && (count  >= MPU9150_FIFO_CHUNK_SIZE) && (count < (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE))) {
+        // special case of a small fifo and nothing cached - just handle as simple read
+
+        if (!I2CRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+            return false;
+
+        if (!I2CRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
+            return false;
+    } else {
+        if (count >= (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE)) {
+            if (m_cacheCount == MPU9150_CACHE_BLOCK_COUNT) {
+                // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+                m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+                if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
+                    m_cacheOut = 0;
+                m_cacheCount--;
+            }
+
+            int blockCount = count / MPU9150_FIFO_CHUNK_SIZE;   // number of chunks in fifo
+
+            if (blockCount > MPU9150_CACHE_SIZE)
+                blockCount = MPU9150_CACHE_SIZE;
+
+            if (!I2CRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE * blockCount,
+                                m_cache[m_cacheIn].data, "Failed to read fifo data"))
+                return false;
+
+            if (!I2CRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
+                return false;
+
+            m_cache[m_cacheIn].count = blockCount;
+            m_cache[m_cacheIn].index = 0;
+
+            m_cacheCount++;
+            if (++m_cacheIn == MPU9150_CACHE_BLOCK_COUNT)
+                m_cacheIn = 0;
+
+        }
+
+        //  now fifo has been read if necessary, get something to process
+
+        if (m_cacheCount == 0)
+            return false;
+
+        memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9150_FIFO_CHUNK_SIZE);
+        memcpy(compassData, m_cache[m_cacheOut].compass, 8);
+
+        m_cache[m_cacheOut].index += MPU9150_FIFO_CHUNK_SIZE;
+
+        if (--m_cache[m_cacheOut].count == 0) {
+            //  this cache block is now empty
+
+            if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
+                m_cacheOut = 0;
+            m_cacheCount--;
+        }
+    }
+
+#else
+
+    if (count > MPU9150_FIFO_CHUNK_SIZE * 40) {
         // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
-        while (count >= 120) {
-            if (!I2CRead(m_slaveAddr, MPU9150_FIFO_R_W, 12, fifoData, "Failed to read fifo data"))
+        while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) {
+            if (!I2CRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
                 return false;
-            count -= 12;
+            count -= MPU9150_FIFO_CHUNK_SIZE;
             m_imuData.timestamp += m_sampleInterval;
         }
     }
 
-    if (count < 12)
+    if (count < MPU9150_FIFO_CHUNK_SIZE)
         return false;
 
-    if (!I2CRead(m_slaveAddr, MPU9150_FIFO_R_W, 12, fifoData, "Failed to read fifo data"))
+    if (!I2CRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
         return false;
 
     if (!I2CRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
         return false;
 
+#endif
+
     RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
     RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);
     RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.3f, false);

RTIMULib/RTIMUMPU9150.h

@@ -22,6 +22,10 @@
 
 #include "RTIMU.h"
 
+//  Define this symbol to use cache mode
+
+#define MPU9150_CACHE_MODE
+
 //  MPU9150 I2C Slave Addresses
 
 #define MPU9150_ADDRESS0            0x68
@@ -102,6 +106,29 @@
 #define AK8975_CNTL                 0x0a                    // control reg
 #define AK8975_ASAX                 0x10                    // start of the fuse ROM data
 
+//  FIFO transfer size
+
+#define MPU9150_FIFO_CHUNK_SIZE     12                      // gyro and accels take 12 bytes
+
+#ifdef MPU9150_CACHE_MODE
+
+//  Cache mode defines
+
+#define MPU9150_CACHE_SIZE          16                      // number of chunks in a block
+#define MPU9150_CACHE_BLOCK_COUNT   16                      // number of cache blocks
+
+typedef struct
+{
+    unsigned char data[MPU9150_FIFO_CHUNK_SIZE * MPU9150_CACHE_SIZE];
+    int count;                                              // number of chunks in the cache block
+    int index;                                              // current index into the cache
+    unsigned char compass[8];                               // the raw compass readings for the block
+
+} MPU9150_CACHE_BLOCK;
+
+#endif
+
+
 class RTIMUMPU9150 : public RTIMU
 {
 public:
@@ -148,6 +175,16 @@ class RTIMUMPU9150 : public RTIMU
     bool m_gyroLearning;                                    // if in learning mode
 
     RTVector3 m_compassAverage;                             // a running average to smooth the mag outputs
+
+#ifdef MPU9150_CACHE_MODE
+
+    MPU9150_CACHE_BLOCK m_cache[MPU9150_CACHE_BLOCK_COUNT]; // the cache itself
+    int m_cacheIn;                                          // the in index
+    int m_cacheOut;                                         // the out index
+    int m_cacheCount;                                       // number of used cache blocks
+
+#endif
+
 };
 
 #endif // _RTIMUMPU9150_H