am dc17e951: Invensense: 6515: update HAL to 5.2.0 RC22

* commit 'dc17e951c1ea1633fb80a9a59931e073f154c924':
  Invensense: 6515: update HAL to 5.2.0 RC22

8 files changed, 147 insertions, 89 deletions

diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp
index 620ea5f..afb6119 100755..100644
--- a/6515/libsensors_iio/MPLSensor.cpp
+++ b/6515/libsensors_iio/MPLSensor.cpp
@@ -589,9 +589,12 @@ void MPLSensor::enable_iio_sysfs(void)
     if (tempFp == NULL) {
         LOGE("HAL:could not open timestamp enable");
     } else {
-        if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) {
+        if(fprintf(tempFp, "%d", 1) < 0) {
             LOGE("HAL:could not enable timestamp");
         }
+        if(fclose(tempFp) < 0) {
+            LOGE("HAL:could not close timestamp");
+        }
     }
 
     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
@@ -600,9 +603,12 @@ void MPLSensor::enable_iio_sysfs(void)
     if (tempFp == NULL) {
         LOGE("HAL:could not open buffer length");
     } else {
-        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) {
+        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
             LOGE("HAL:could not write buffer length");
         }
+        if (fclose(tempFp) < 0) {
+            LOGE("HAL:could not close buffer length");
+        }
     }
 
     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
@@ -611,9 +617,12 @@ void MPLSensor::enable_iio_sysfs(void)
     if (tempFp == NULL) {
         LOGE("HAL:could not open chip enable");
     } else {
-        if (fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) {
+        if (fprintf(tempFp, "%d", 1) < 0) {
             LOGE("HAL:could not write chip enable");
         }
+        if (fclose(tempFp) < 0) {
+            LOGE("HAL:could not close chip enable");
+        }
     }
 
     inv_get_iio_device_node(iio_device_node);
@@ -809,11 +818,14 @@ void MPLSensor::loadDMP(void)
             if(fptr == NULL) {
                 LOGE("HAL:could not open dmp_firmware");
             } else {
-                if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) {
+                if (inv_load_dmp(fptr) < 0) {
                     LOGE("HAL:load DMP failed");
                 } else {
                     LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
                 }
+                if (fclose(fptr) < 0) {
+                    LOGE("HAL:could not close dmp firmware");
+                }
             }
         } else {
             LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded");
@@ -837,7 +849,7 @@ void MPLSensor::inv_get_sensors_orientation(void)
         int om[9];
         if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
                &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
-               &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) {
+               &om[6], &om[7], &om[8]) < 0) {
             LOGE("HAL:Could not read gyro mounting matrix");
         } else {
             LOGV_IF(EXTRA_VERBOSE,
@@ -855,6 +867,9 @@ void MPLSensor::inv_get_sensors_orientation(void)
             mGyroOrientation[7] = om[7];
             mGyroOrientation[8] = om[8];
         }
+        if (fclose(fptr) < 0) {
+            LOGE("HAL:Could not close gyro mounting matrix");
+        }
     }
 
     // get accel orientation
@@ -865,7 +880,7 @@ void MPLSensor::inv_get_sensors_orientation(void)
         int om[9];
         if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
             &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
-            &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) {
+            &om[6], &om[7], &om[8]) < 0) {
             LOGE("HAL:could not read accel mounting matrix");
         } else {
             LOGV_IF(EXTRA_VERBOSE,
@@ -883,6 +898,9 @@ void MPLSensor::inv_get_sensors_orientation(void)
            mAccelOrientation[7] = om[7];
            mAccelOrientation[8] = om[8];
         }
+        if (fclose(fptr) < 0) {
+            LOGE("HAL:could not close accel mounting matrix");
+        }
     }
 }
 
@@ -5734,24 +5752,6 @@ int MPLSensor::flush(int handle)
 
     LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle);
 
-#if 0
-    switch (what) {
-    case Gyro:
-    case RawGyro:
-    case Accelerometer:
-    case MagneticField:
-    case RawMagneticField:
-    case Pressure:
-    case GameRotationVector:
-    case StepDetector:
-        LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle);
-        break;
-    default:
-        LOGE("sensor (handle %d) is not supported in batch or flush mode", handle);
-        return -EINVAL;
-    }
-#endif
-
     if (((what != StepDetector) && (!(mEnabled & (1 << what)))) ||
         ((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) {
         LOGE_IF(ENG_VERBOSE, "HAL: flush - sensor %s not enabled", sname.string());
@@ -5769,20 +5769,19 @@ int MPLSensor::flush(int handle)
     status = read_sysfs_int(mpu.flush_batch, &res);
 
     if (status < 0)
-        return status;
+        LOGE("HAL: flush - error invoking flush_batch");
 
     /* driver returns 0 if FIFO is empty */
     /* ensure we return status zero      */
     if (res == 0) {
         LOGI("HAL: flush - no data in FIFO");
-        status = 0;
     }
 
     mFlushSensorEnabledVector.push_back(handle);
     LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushSensorEnabledVector=%d res=%d", handle, res);
 
     mFlushBatchSet = 0;
-    return status;
+    return 0;
 }
 
 int MPLSensor::selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask)
@@ -5945,10 +5944,16 @@ int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
             if (fp == NULL) {
                 LOGE("HAL:cannot open pedometer_steps");
             } else {
-                if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) {
+                if (fscanf(fp, "%lld\n", &stepCount) < 0) {
                     LOGW("HAL:cannot read pedometer_steps");
+                    if (fclose(fp) < 0) {
+                       LOGW("HAL:cannot close pedometer_steps");
+                    }
                     return 0;
                 }
+                if (fclose(fp) < 0) {
+                       LOGW("HAL:cannot close pedometer_steps");
+                }
             }
 
             /* return event onChange only */
@@ -5963,10 +5968,17 @@ int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
             if (fp == NULL) {
                 LOGE("HAL:cannot open pedometer_counter");
             } else{
-                if (fscanf(fp, "%lld\n", &stepCountTs) < 0 || fclose(fp) < 0) {
+                if (fscanf(fp, "%lld\n", &stepCountTs) < 0) {
                     LOGE("HAL:cannot read pedometer_counter");
+                    if (fclose(fp) < 0) {
+                        LOGE("HAL:cannot close pedometer_counter");
+                    }
                     return 0;
                 }
+                if (fclose(fp) < 0) {
+                        LOGE("HAL:cannot close pedometer_counter");
+                        return 0;
+                }
             }
             mScEvents.timestamp = stepCountTs;
 
@@ -6021,9 +6033,12 @@ int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count)
         LOGE("HAL:cannot open event_smd");
         return 0;
     } else {
-        if (fscanf(fp, "%d\n", &significantMotion) < 0 || fclose(fp) < 0) {
+        if (fscanf(fp, "%d\n", &significantMotion) < 0) {
             LOGE("HAL:cannot read event_smd");
         }
+        if (fclose(fp) < 0) {
+            LOGE("HAL:cannot close event_smd");
+        }
     }
 
     if(mDmpSignificantMotionEnabled && count > 0) {
@@ -6041,6 +6056,9 @@ int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count)
             /* reset smd state */
             mDmpSignificantMotionEnabled = 0;
             mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
+
+            /* auto disable this sensor */
+            enableDmpSignificantMotion(0);
         }
     }
 
@@ -6123,11 +6141,14 @@ void MPLSensor::setInitial6QuatValue()
     if(fptr == NULL) {
         LOGE("HAL:could not open six_axis_q_value");
     } else {
-        if (fwrite(quat, 1, length, fptr) != length || fclose(fptr) < 0) {
+        if (fwrite(quat, 1, length, fptr) != length) {
            LOGE("HAL:write six axis q value failed");
         } else {
             mInitial6QuatValueAvailable = 0;
         }
+        if (fclose(fptr) < 0) {
+            LOGE("HAL:could not close six_axis_q_value");
+        }
     }
 
     return;
diff --git a/6515/libsensors_iio/libmllite.so b/6515/libsensors_iio/libmllite.so
index 4a7e0b8..252aa37 100644..100755
--- a/6515/libsensors_iio/libmllite.so
+++ b/6515/libsensors_iio/libmllite.so
diff --git a/6515/libsensors_iio/libmplmpu.so b/6515/libsensors_iio/libmplmpu.so
index cfd0958..a50bdfb 100644..100755
--- a/6515/libsensors_iio/libmplmpu.so
+++ b/6515/libsensors_iio/libmplmpu.so
diff --git a/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so
index 4a7e0b8..252aa37 100755
--- a/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so
+++ b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
index 7715f47..fc7dc50 100644
--- a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
+++ b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -30,7 +30,15 @@
 #define LOADDMP_LOG MPL_LOGI
 
 #define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
+#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
 #define DMP_CODE_SIZE 2950
+#define FIFO_SIZE     1024
+#define RESERVED_SIZE 32
+#define MEMORY_SIZE   1024*4
+#define DMP_SIZE_AVAILABLE (MEMORY_SIZE - FIFO_SIZE - RESERVED_SIZE - DMP_CODE_SIZE)
+#if DMP_SIZE_AVAILABLE < 0
+#error "DMP image size is larger than available memory"
+#endif
 
 static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     /* bank # 0 */
@@ -45,9 +53,9 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x36, 0x66, 0x66, 0x66,
     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x1f, 0xdb, 0xe2, 0xb9, 0x3b, 0x7b, 0x11, 0x7b, 0x32, 0x57, 0x83, 0x7b,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00,
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x1f, 0xdb, 0xe2, 0xb9, 0x3b, 0x7b, 0x11, 0x7b, 0x32, 0x57, 0x83, 0x7b,
     /* bank # 1 */
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x51, 0xeb, 0x85, 0x3f, 0xae, 0x14, 0x7b,
     0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -167,12 +175,12 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0xd8, 0xf1, 0xbb, 0xb3, 0xb7, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80,
     0x98, 0xa7, 0x20, 0xd8, 0xf1, 0xb3, 0xb7, 0xbb, 0x87, 0xaa, 0xd0, 0xc1, 0xa7, 0x81, 0x97, 0x62,
     0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c,
-    0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xf1, 0xb0, 0xb4, 0xb8, 0x87, 0x98, 0xad, 0x20, 0xad,
+    0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xf1, 0xb0, 0xb4, 0xb8, 0x87, 0x98, 0xaf, 0x20, 0xaf,
     /* bank # 8 */
-    0x83, 0x9d, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0x8e, 0xb4, 0x94, 0x01, 0x29, 0x51, 0x79,
-    0x97, 0xae, 0xf1, 0x28, 0x4c, 0x6c, 0x8d, 0x0c, 0x9e, 0x18, 0x8e, 0x78, 0x87, 0x98, 0xad, 0x20,
-    0xad, 0x83, 0x9d, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0xb2, 0x81, 0xb4, 0x94, 0x01, 0x9d,
-    0x29, 0x51, 0x79, 0x97, 0xba, 0xa1, 0xf1, 0x28, 0x4c, 0x6c, 0xb0, 0x8d, 0x0c, 0xb6, 0x91, 0x18,
+    0x83, 0x9f, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0x8e, 0xb4, 0x94, 0x01, 0x29, 0x51, 0x79,
+    0x97, 0xae, 0xf1, 0x28, 0x4c, 0x6c, 0x8f, 0x0c, 0x9e, 0x18, 0x8e, 0x78, 0x87, 0x98, 0xaf, 0x20,
+    0xaf, 0x83, 0x9f, 0x22, 0xb7, 0x93, 0xf0, 0x31, 0x31, 0x20, 0xb2, 0x81, 0xb4, 0x94, 0x01, 0x9f,
+    0x29, 0x51, 0x79, 0x97, 0xba, 0xa1, 0xf1, 0x28, 0x4c, 0x6c, 0xb0, 0x8f, 0x0c, 0xb6, 0x91, 0x18,
     0xb2, 0x81, 0x78, 0xf1, 0xb0, 0x87, 0xb4, 0x98, 0xba, 0xab, 0x20, 0xab, 0xb2, 0x86, 0xb6, 0x9b,
     0x02, 0xb7, 0x93, 0xf0, 0x11, 0x11, 0x00, 0x84, 0xb6, 0x97, 0x01, 0x29, 0x51, 0x79, 0xb4, 0x97,
     0xa4, 0xf1, 0x28, 0x4c, 0x6c, 0x8b, 0x0c, 0xb6, 0x94, 0x18, 0xb2, 0x84, 0x78, 0xbb, 0xa3, 0xb3,
@@ -180,15 +188,15 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0xd9, 0xa3, 0xd0, 0xf8, 0xf5, 0x8e, 0x9e, 0xb9, 0xa3, 0x78, 0xf1, 0xb1, 0x8a, 0xb5, 0x93, 0x45,
     0xb2, 0x8c, 0x2e, 0xb1, 0x8a, 0x54, 0x83, 0xaa, 0xd0, 0xc6, 0xf5, 0xb2, 0x81, 0xb6, 0x91, 0xa3,
     0x78, 0xf1, 0xb1, 0x8a, 0xb5, 0x93, 0x65, 0xb2, 0x8c, 0x2e, 0xb1, 0x8a, 0x74, 0x83, 0xaa, 0xd0,
-    0xc7, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xad, 0xc6, 0xf9, 0xa3, 0xb4, 0x9d, 0x41, 0xd9, 0xf1, 0xb1,
+    0xc7, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xaf, 0xc6, 0xf9, 0xa3, 0xb4, 0x9f, 0x41, 0xd9, 0xf1, 0xb1,
     0x8a, 0xba, 0xa6, 0xc5, 0xc7, 0xf1, 0xb2, 0xb6, 0xb9, 0x86, 0x96, 0xa3, 0x50, 0xb1, 0x83, 0xb5,
     0x93, 0x04, 0x2c, 0xf3, 0xb8, 0xa3, 0xd0, 0xf8, 0xf9, 0xd1, 0xdb, 0xf1, 0xb6, 0x9c, 0xb9, 0xa3,
-    0xd0, 0x56, 0xd8, 0xf2, 0xb0, 0x83, 0xb4, 0x9d, 0xb8, 0xa3, 0x41, 0xdb, 0xf3, 0x97, 0x49, 0xf1,
-    0xb1, 0x83, 0xb6, 0x9c, 0xb9, 0xa3, 0xd0, 0x5e, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xa3, 0xb4, 0x9d,
+    0xd0, 0x56, 0xd8, 0xf2, 0xb0, 0x83, 0xb4, 0x9f, 0xb8, 0xa3, 0x41, 0xdb, 0xf3, 0x97, 0x49, 0xf1,
+    0xb1, 0x83, 0xb6, 0x9c, 0xb9, 0xa3, 0xd0, 0x5e, 0xd8, 0xf2, 0xb0, 0x83, 0xb8, 0xa3, 0xb4, 0x9f,
     /* bank # 9 */
     0x41, 0xd9, 0xf3, 0xa3, 0xd0, 0xde, 0xf8, 0xf1, 0xb1, 0x83, 0xb6, 0x96, 0xb9, 0xa3, 0xdb, 0x69,
     0xf3, 0xb8, 0xa3, 0xd0, 0xde, 0xf1, 0xb3, 0x86, 0xba, 0xa0, 0xc0, 0xd8, 0xf2, 0xb0, 0x83, 0xb8,
-    0xa3, 0xb4, 0x9d, 0x41, 0xd9, 0xd0, 0xde, 0xd8, 0xf3, 0xbb, 0xb3, 0xb7, 0x90, 0xa2, 0x82, 0x00,
+    0xa3, 0xb4, 0x9f, 0x41, 0xd9, 0xd0, 0xde, 0xd8, 0xf3, 0xbb, 0xb3, 0xb7, 0x90, 0xa2, 0x82, 0x00,
     0xf2, 0x10, 0xf1, 0xba, 0xa2, 0xd0, 0xfa, 0xf2, 0xab, 0xc2, 0xf8, 0xf9, 0xd1, 0xf1, 0xb9, 0xa4,
     0xd9, 0xde, 0xda, 0xf8, 0xd8, 0xf2, 0xbb, 0xaf, 0x80, 0x92, 0x50, 0x8f, 0x0d, 0xdb, 0xf1, 0xb1,
     0x84, 0xb5, 0x94, 0x21, 0xd9, 0xf5, 0xb3, 0x85, 0xb7, 0x95, 0xb9, 0xa3, 0x78, 0xf1, 0xb1, 0x80,
diff --git a/6515/libsensors_iio/software/core/mllite/mpl.c b/6515/libsensors_iio/software/core/mllite/mpl.c
index 085e717..79631e8 100644
--- a/6515/libsensors_iio/software/core/mllite/mpl.c
+++ b/6515/libsensors_iio/software/core/mllite/mpl.c
@@ -46,7 +46,7 @@ inv_error_t inv_init_mpl(void)
     return INV_SUCCESS;
 }
 
-const char ml_ver[] = "InvenSense MA 5.2.0 K RC20";
+const char ml_ver[] = "InvenSense MA 5.2.0 K RC22";
 
 /**
  *  @brief  used to get the MPL version.
diff --git a/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
index cfd0958..a50bdfb 100755
--- a/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
+++ b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c
index 32bb7ad..0186041 100644
--- a/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c
+++ b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c
@@ -62,9 +62,15 @@ static void handle_smd() {
 
 static void pedo_print()
 {
-	printf("steps=%lld, time=%lld\n",
+	struct timespec aa;
+	unsigned long long t;
+
+	clock_gettime(CLOCK_REALTIME, &aa);
+	t = (unsigned long long)aa.tv_sec * 1000000000 + aa.tv_nsec;
+	printf("steps=%lld, time=%lld, system=%lld\n",
 		read_sysfs_poslonglong("pedometer_steps", dev_dir_name),
-		read_sysfs_poslonglong("pedometer_time", dev_dir_name));
+		read_sysfs_poslonglong("pedometer_time", dev_dir_name),
+		t);
 }
 
 struct dmp_struct event_file[] = {
@@ -155,11 +161,11 @@ static void inv_set_rate()
 	ret = write_sysfs_int_and_verify("accel_rate", dev_dir_name, 5);
 	ret = write_sysfs_int_and_verify("gyro_rate", dev_dir_name, 5);
 	if (has_compass)
-		ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 50);
+		ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 10);
 	if (has_pressure)
-		ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 15);
+		ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 30);
+	ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 5);
+	ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 5);
 	ret = write_sysfs_int_and_verify("three_axes_q_rate", dev_dir_name, 5);
 }
 
@@ -186,7 +192,7 @@ static int setup_offset_and_bias()
 		printf("write accel y offset failed.\n");
 	ret = write_sysfs_int_and_verify("in_anglvel_z_offset", dev_dir_name, 0);
 	if (ret < 0)
-		printf("write accel z offset failed.\n"); 
+		printf("write accel z offset failed.\n");
 
 	ret = write_sysfs_int_and_verify("in_accel_x_dmp_bias", dev_dir_name, 0);
 	if (ret < 0)
@@ -226,17 +232,17 @@ static void setup_dmp(char *dev_path){
 		return;
 	ret = write_sysfs_int("in_anglvel_scale", dev_path, 3);
 	if (ret < 0)
-		return;	
+		return;
 	ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
 	if (ret < 0)
-		return;	
+		return;
 	ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0);
 	if (ret < 0)
 		return;
 	sprintf(dmp_path, "%s/dmp_firmware", dev_path);
 	if ((fd = fopen(dmp_path, "wb")) < 0 ) {
 		perror("dmp fail");
-	}	
+	}
 	inv_load_dmp(fd);
 	fclose(fd);
 	printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
@@ -247,16 +253,12 @@ static void setup_dmp(char *dev_path){
 	if (ret < 0)
 		return;
 	/* selelct which event to enable and interrupt on/off here */
-	//enable_glu(sysfs_path, 0);
-	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0);
-	if (ret < 0)
-		return;
 	ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1);
 	ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1);
 
 	ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1);
-		write_sysfs_int64("pedometer_steps", sysfs_path, 0x3ffffffff);
-		write_sysfs_int64("pedometer_time", sysfs_path, 0xffffffff);
+		write_sysfs_int64("pedometer_steps", sysfs_path, 0);
+		write_sysfs_int64("pedometer_time", sysfs_path, 0);
 	if (ret < 0)
 		return;
 
@@ -316,9 +318,9 @@ static void *get_dmp_event(void *param) {
 				fscanf(fp, "%d\n", &data);
 				event_file[i].action(&event_file[i], data);
 			}
-		}						
+		}
 	}
-	
+
 	return 0;
 }
 
@@ -419,14 +421,14 @@ static int enable_enable(int on){
 	int ret;
 
 	if (0 == on) {
-		pthread_mutex_lock(&data_switch_lock);
+		//pthread_mutex_lock(&data_switch_lock);
 	}
 	ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, on);
 	if (ret < 0)
 		printf("write enable failed\n");
 
 	if (on) {
-		pthread_mutex_unlock(&data_switch_lock);
+		//pthread_mutex_unlock(&data_switch_lock);
 	}
 
 	return 0;
@@ -469,8 +471,8 @@ static void dmp_event_control(on){
 	inv_set_rate();
 
 	//ret = write_sysfs_int_and_verify("batchmode_wake_fifo_full_on", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 5000);
-//	ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0);
+	ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 10000);
+	ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0);
 	//ret = write_sysfs_int_and_verify("smd_delay_threshold", dev_dir_name, 10);
 	if (ret < 0)
 		return;
@@ -507,8 +509,8 @@ static int run_enable_sequence()
 	if (!g && !a)
 		a = true;
 
-	//g = true;
-	//a = true;
+	g = true;
+	a = true;
 	/*disable the master enable */
 	enable_enable(0);
 	if(g) {
@@ -521,8 +523,8 @@ static int run_enable_sequence()
 			enable_quaternion(1);
 			enable_gyro_output(1);
 		}
-	//	enable_quaternion(0);
-	//	enable_gyro_output(0);
+		enable_quaternion(1);
+		enable_gyro_output(0);
 
 	} else {
 		enable_gyro(0);
@@ -531,7 +533,7 @@ static int run_enable_sequence()
 	}
 	if(a) {
 		enable_accel(1);
-		enable_accel_output(1);
+		enable_accel_output(0);
 	} else {
 		enable_accel(0);
 		enable_accel_output(0);
@@ -542,7 +544,7 @@ static int run_enable_sequence()
 		else
 			enable_compass(0);
 		enable_compass(counter%2);
-		//enable_compass(0);
+		enable_compass(0);
 	}
 	if (has_pressure) {
 		if(rand()%2)
@@ -550,12 +552,12 @@ static int run_enable_sequence()
 		else
 			enable_pressure(0);
 		enable_pressure(counter%3);
-		//enable_pressure(0);
+		enable_pressure(0);
 	}
 	enable_step_detector(1);
 	enable_step_indicator(1);
-	//enable_step_detector(0);
-	//enable_step_indicator(0);
+	enable_step_detector(0);
+	enable_step_indicator(0);
 
 	write_dmp_event(0);
 
@@ -567,7 +569,6 @@ static int run_enable_sequence()
 	first_flag = 1;
 	/*enable the master enable */
 	enable_enable(1);
-	//write_sysfs_string_and_verify("wake_unlock", "/sys/power/", "hack");
 	if (enable_random_delay)
 		random_delay();
 	else {
@@ -600,6 +601,15 @@ static int run_disable_sequence() {
 		sleep(disable_delay);
 	}
 
+	if (has_pressure) {
+		if(rand()%2)
+			enable_pressure(1);
+		else
+			enable_pressure(0);
+		enable_pressure(counter%3);
+		enable_pressure(1);
+	}
+
 	return 0;
 }
 static int run_dmp_off() {
@@ -613,8 +623,7 @@ static int run_dmp_off() {
 
 	g = true;
 	a = true;
-//	a = false;
-//	g = false;
+	a = false;
 	/*disable the master enable */
 	enable_enable(0);
 	if(g) {
@@ -644,7 +653,7 @@ static int run_dmp_off() {
 		else
 			enable_compass(0);
 		enable_compass(counter%2);
-		enable_compass(1);
+		enable_compass(0);
 	}
 	if (has_pressure) {
 		if(rand()%2)
@@ -654,22 +663,24 @@ static int run_dmp_off() {
 		enable_pressure(counter%3);
 		enable_pressure(1);
 	}
+	printf("111111111111111\n");
 
-	write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,100);
+	write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,15);
+	write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
 	first_flag = 1;
 	/*enable the master enable */
-	enable_enable(1);	
-	sleep(2);
+	enable_enable(1);
+	//sleep(2);
 
 	return 0;
-}	
+}
 static void *control_switch(void *param)
 {
 	while(1) {
 		run_enable_sequence();
-		run_dmp_off();
+		//run_dmp_off();
 		printf("sleeping\n");
-		sleep(1000);
+		//sleep(1000);
 		run_disable_sequence();
 	}
 	return 0;
@@ -686,17 +697,21 @@ void get_sensor_data(char *d, short *sensor)
 static void *read_data(void *param)
 {
 	char *buffer_access;
-	char data[1048], *dptr, tmp[24];
+	char data[2048], *dptr, tmp[24];
 	short sensor[3];
 	int q[3], i, ind, left_over_size, buf_size;
 	int ret, fp,read_size;
 	unsigned short hdr;
 	bool done_flag;
+	struct timespec ts_1;
+	unsigned long long t0, t1;
+	int g_count, sq_count;
 
 #define PRESSURE_HDR             0x8000
 #define ACCEL_HDR                0x4000
 #define GYRO_HDR                 0x2000
 #define COMPASS_HDR              0x1000
+#define COMPASS_HDR_2            0x1800
 #define LPQUAT_HDR               0x0800
 #define SIXQUAT_HDR              0x0400
 #define PEDQUAT_HDR              0x0200
@@ -720,8 +735,18 @@ static void *read_data(void *param)
 		goto error_open_buffer_access;
 	}
 	ind = 0;
-	
+
+	clock_gettime(CLOCK_REALTIME, &ts_1);
+	t0 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec;
 	while(1) {
+
+		clock_gettime(CLOCK_REALTIME, &ts_1);
+		t1 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec;
+		//printf("diff=%lld, a_count=%d, sq_count=%d\n", (t1-t0), g_count, sq_count);
+		g_count = 0;
+		sq_count = 0;
+		t0 = t1;
+
 		struct pollfd pfd = {
 			.fd = fp,
 			.events = POLLIN,
@@ -731,7 +756,7 @@ static void *read_data(void *param)
 		if (left_over_size > 0)
 			memcpy(data, tmp, left_over_size);
 		dptr = data + left_over_size;
-		read_size = read(fp,  dptr, 1024);
+		read_size = read(fp,  dptr, 2000);
 		printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size);
 		if (read_size <= 0) {
 			printf("Wrong size=%d\n", read_size);
@@ -740,7 +765,7 @@ static void *read_data(void *param)
 		}
 		ind = read_size + left_over_size;
 		dptr = data;
-		printf("ind=%d\n", ind);
+		//printf("ind=%d\n", ind);
 		buf_size = ind - (dptr - data);
 		done_flag = false;
 
@@ -769,6 +794,7 @@ static void *read_data(void *param)
 				if (buf_size >= 16) {
 					get_sensor_data(dptr, sensor);
 					dptr += 8;
+					g_count++;
 					printf("G:%d, %d, %d,  %lld\n", sensor[0], sensor[1], sensor[2],   *(long long *)dptr);
 				} else
 					done_flag = true;
@@ -807,6 +833,7 @@ static void *read_data(void *param)
 					q[1] = *(int *)(dptr);
 					q[2] = *(int *)(dptr + 4);
 					dptr += 8;
+					sq_count++;
 					printf("SIXQ:%d, %d, %d,  %lld\n", q[0], q[1], q[2],   *(long long *)dptr);
 				}  else
 					done_flag = true;
@@ -825,6 +852,8 @@ static void *read_data(void *param)
 					printf("emptry marker !!!!!!!!!!!\n");
 				} else if (hdr == END_MARKER) {
 					printf("end marker !!!!!\n");
+				} else if (hdr == COMPASS_HDR_2) {
+					printf("bad compass data\n");
 				} else {
 					dptr +=8;
 					printf("%lld\n", *(long long *)dptr);
@@ -992,7 +1021,7 @@ int main(int argc, char **argv)
 		goto error_free_buf_dir_name;
 	}
 	/* Setup ring buffer parameters */
-	/* length must be even number because iio_store_to_sw_ring is expecting 
+	/* length must be even number because iio_store_to_sw_ring is expecting
 		half pointer to be equal to the read pointer, which is impossible
 		when buflen is odd number. This is actually a bug in the code */
 	ret = write_sysfs_int("length", buf_dir_name, buf_len*2);