Merge tag 'android-4.3_r2.1' into cm-10.2cm-10.2.1 cm-10.2.0 cm-10.2-M1 stable/cm-10.2 cm-10.2

Android 4.3 release 2.1
author: Ricardo Cerqueira <cyanogenmod@cerqueira.org> 2013-07-24 23:01:58 +0100
committer: Ricardo Cerqueira <cyanogenmod@cerqueira.org> 2013-07-24 23:01:58 +0100
commit: de880a60e91e342912110aecc5f15385d791aa2a (patch)
tree: d76c01ea1e0b220d8136d5f819da146946a958c7
parent: 33ce91b37062fa63af192f5643de93f3beebe854 (diff)
parent: 60c042a376b34a7f4343752b96c1bc0bf4da31cc (diff)
download: android_hardware_invensense-cm-10.2.0.tar.gz
android_hardware_invensense-cm-10.2.0.tar.bz2
android_hardware_invensense-cm-10.2.0.zip
45 files changed, 1795 insertions, 1024 deletions
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp
index a36a565..e766a7f 100644
--- a/libsensors/MPLSensor.cpp
+++ b/libsensors/MPLSensor.cpp
@@ -142,7 +142,7 @@ void setCallbackObject(MPLSensor* gbpt)
 
 MPLSensor::MPLSensor() :
     SensorBase(NULL, NULL),
-            mMpuAccuracy(0), mNewData(0),
+            mNewData(0),
             mDmpStarted(false),
             mMasterSensorMask(INV_ALL_SENSORS),
             mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
@@ -220,30 +220,22 @@ MPLSensor::MPLSensor() :
     mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
     mPendingEvents[RotationVector].sensor = ID_RV;
     mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
-    mPendingEvents[RotationVector].acceleration.status
-            = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
     mPendingEvents[LinearAccel].sensor = ID_LA;
     mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
-    mPendingEvents[LinearAccel].acceleration.status
-            = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Gravity].version = sizeof(sensors_event_t);
     mPendingEvents[Gravity].sensor = ID_GR;
     mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
-    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Gyro].version = sizeof(sensors_event_t);
     mPendingEvents[Gyro].sensor = ID_GY;
     mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
-    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
     mPendingEvents[Accelerometer].sensor = ID_A;
     mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
-    mPendingEvents[Accelerometer].acceleration.status
-            = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
     mPendingEvents[MagneticField].sensor = ID_M;
@@ -508,7 +500,6 @@ void MPLSensor::initMPL()
         ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
     }
 
-    mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM;
     setupCallbacks();
 
 }
@@ -578,7 +569,6 @@ void MPLSensor::cbOnMotion(uint16_t val)
     FUNC_LOG;
     //after the first no motion, the gyro should be calibrated well
     if (val == 2) {
-        mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
         if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) {
             //if gyros are on and we got a no motion, set a flag
             // indicating that the cal file can be written.
@@ -609,7 +599,6 @@ void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask,
     s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0;
     s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0;
     s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0;
-    s->gyro.status = mMpuAccuracy;
     if (res == INV_SUCCESS)
         *pending_mask |= (1 << index);
 }
@@ -625,7 +614,6 @@ void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask,
     s->acceleration.v[1] = s->acceleration.v[1] * 9.81;
     s->acceleration.v[2] = s->acceleration.v[2] * 9.81;
     //ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]);
-    s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
     if (res == INV_SUCCESS)
         *pending_mask |= (1 << index);
 }
@@ -707,9 +695,6 @@ void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask,
     s->gyro.v[1] = quat[2];
     s->gyro.v[2] = quat[3];
 
-    s->gyro.status
-            = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy
-                                                            : estimateCompassAccuracy());
 }
 
 void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask,
@@ -721,7 +706,6 @@ void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask,
     s->gyro.v[0] *= 9.81;
     s->gyro.v[1] *= 9.81;
     s->gyro.v[2] *= 9.81;
-    s->gyro.status = mMpuAccuracy;
     if (res == INV_SUCCESS)
         *pending_mask |= (1 << index);
 }
@@ -735,7 +719,6 @@ void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask,
     s->gyro.v[0] *= 9.81;
     s->gyro.v[1] *= 9.81;
     s->gyro.v[2] *= 9.81;
-    s->gyro.status = mMpuAccuracy;
     if (res == INV_SUCCESS)
         *pending_mask |= (1 << index);
 }
@@ -1364,4 +1347,3 @@ void MPLSensor::fillLinearAccel(struct sensor_t *list)
     list[Gravity].maxRange = list[Accelerometer].maxRange;
     return;
 }
-
diff --git a/libsensors/MPLSensor.h b/libsensors/MPLSensor.h
index 2b1571d..5b5d121 100644
--- a/libsensors/MPLSensor.h
+++ b/libsensors/MPLSensor.h
@@ -87,7 +87,6 @@ protected:
     void calcOrientationSensor(float *Rx, float *Val);
     int estimateCompassAccuracy();
 
-    int mMpuAccuracy; //global storage for the current accuracy status
     int mNewData; //flag indicating that the MPL calculated new output values
     int mDmpStarted;
     long mMasterSensorMask;
diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk
index c3b2003..af52ea6 100644
--- a/libsensors_iio/Android.mk
+++ b/libsensors_iio/Android.mk
@@ -23,8 +23,11 @@ include $(CLEAR_VARS)
 LOCAL_MODULE := libinvensense_hal
 
 LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
 
 LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
+# Comment out for ICS. Affects Android LOG macros.
+LOCAL_CFLAGS += -DANDROID_JELLYBEAN
 
 ifeq ($(ENG_BUILD),1)
 ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
@@ -33,8 +36,8 @@ endif
 ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
 LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
 endif
-ifeq ($(COMPILE_COMPASS_YAS530),1)
-LOCAL_CFLAGS += -DCOMPASS_YAS530
+ifeq ($(COMPILE_COMPASS_YAS53x),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS53x
 endif
 ifeq ($(COMPILE_COMPASS_AK8975),1)
 LOCAL_CFLAGS += -DCOMPASS_AK8975
@@ -64,7 +67,7 @@ LOCAL_SHARED_LIBRARIES += libutils
 LOCAL_SHARED_LIBRARIES += libdl
 LOCAL_SHARED_LIBRARIES += libmllite
 
-#Additions for SysPed
+# Additions for SysPed
 LOCAL_SHARED_LIBRARIES += libmplmpu
 LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
 LOCAL_CPPFLAGS += -DLINUX=1
@@ -76,6 +79,7 @@ include $(CLEAR_VARS)
 LOCAL_MODULE := libmplmpu
 LOCAL_SRC_FILES := libmplmpu.so
 LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
 LOCAL_MODULE_SUFFIX := .so
 LOCAL_MODULE_CLASS := SHARED_LIBRARIES
 LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
@@ -86,6 +90,7 @@ include $(CLEAR_VARS)
 LOCAL_MODULE := libmllite
 LOCAL_SRC_FILES := libmllite.so
 LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_OWNER := invensense
 LOCAL_MODULE_SUFFIX := .so
 LOCAL_MODULE_CLASS := SHARED_LIBRARIES
 LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
diff --git a/libsensors_iio/CompassSensor.IIO.9150.cpp b/libsensors_iio/CompassSensor.IIO.9150.cpp
index 7d99af9..ce96564 100644
--- a/libsensors_iio/CompassSensor.IIO.9150.cpp
+++ b/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -35,8 +35,8 @@
 
 #define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
 
-#if defined COMPASS_YAS530
-#   warning "Invensense compass cal with YAS530 IIO on primary bus"
+#if defined COMPASS_YAS53x
+#   warning "Invensense compass cal with YAS53x IIO on secondary bus"
 #   define USE_MPL_COMPASS_HAL          (1)
 #   define COMPASS_NAME                 "INV_YAS530"
 #elif defined COMPASS_AK8975
@@ -143,22 +143,12 @@ int CompassSensor::enable(int32_t handle, int en)
     mEnable = en;
     int res;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
-            en, compassSysFs.compass_enable, getTimestamp());
     res = write_sysfs_int(compassSysFs.compass_enable, en);
     LOGE_IF(res < 0, "HAL:enable compass failed");
 
     if (en) {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
-                en, compassSysFs.compass_x_fifo_enable, getTimestamp());
         res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
-
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
-                en, compassSysFs.compass_y_fifo_enable, getTimestamp());
         res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
-
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
-                en, compassSysFs.compass_z_fifo_enable, getTimestamp());
         res = write_sysfs_int(compassSysFs.compass_z_fifo_enable, en);
     }
 
@@ -313,10 +303,10 @@ void CompassSensor::fillList(struct sensor_t *list)
             return;
         }
         if(!strcmp(compass, "INV_YAS530")) {
-            list->maxRange = COMPASS_YAS530_RANGE;
-            list->resolution = COMPASS_YAS530_RESOLUTION;
-            list->power = COMPASS_YAS530_POWER;
-            list->minDelay = COMPASS_YAS530_MINDELAY;
+            list->maxRange = COMPASS_YAS53x_RANGE;
+            list->resolution = COMPASS_YAS53x_RESOLUTION;
+            list->power = COMPASS_YAS53x_POWER;
+            list->minDelay = COMPASS_YAS53x_MINDELAY;
             return;
         }
         if(!strcmp(compass, "INV_AMI306")) {
@@ -389,8 +379,8 @@ int CompassSensor::inv_init_sysfs_attributes(void)
     sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
 #endif
 
-#if 0
-    // test print sysfs paths   
+#if SYSFS_VERBOSE
+    // test print sysfs paths
     dptr = (char**)&compassSysFs;
     for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
         LOGE("HAL:sysfs path: %s", *dptr++);
diff --git a/libsensors_iio/InputEventReader.h b/libsensors_iio/InputEventReader.h
index f0ccc63..5c752db 100644
--- a/libsensors_iio/InputEventReader.h
+++ b/libsensors_iio/InputEventReader.h
@@ -22,6 +22,8 @@
 #include <sys/cdefs.h>
 #include <sys/types.h>
 
+#include "SensorBase.h"
+
 /*****************************************************************************/
 
 struct input_event;
diff --git a/libsensors_iio/MPLSensor.cpp b/libsensors_iio/MPLSensor.cpp
index 90051cd..725a4fc 100644
--- a/libsensors_iio/MPLSensor.cpp
+++ b/libsensors_iio/MPLSensor.cpp
@@ -39,6 +39,7 @@
 #include "MPLSensor.h"
 #include "MPLSupport.h"
 #include "sensor_params.h"
+#include "local_log_def.h"
 
 #include "invensense.h"
 #include "invensense_adv.h"
@@ -89,7 +90,7 @@ static struct sensor_t sSensorList[] =
     {"MPL Gyroscope", "Invensense", 1,
      SENSORS_GYROSCOPE_HANDLE,
      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
-    {"MPL Raw Gyro", "Invensense", 1,
+    {"MPL Raw Gyroscope", "Invensense", 1,
      SENSORS_RAW_GYROSCOPE_HANDLE,
      SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
     {"MPL Accelerometer", "Invensense", 1,
@@ -143,14 +144,14 @@ MPLSensor* getCallbackObject() {
 static FILE *logfile = NULL;
 #endif
 
+pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
+
 /*******************************************************************************
  * MPLSensor class implementation
  ******************************************************************************/
 
-// following extended initializer list would only be available with -std=c++11 or -std=gnu+11
-MPLSensor::MPLSensor(CompassSensor *compass)
+MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
                        : SensorBase(NULL, NULL),
-                         mMplSensorInitialized(false),
                          mNewData(0),
                          mMasterSensorMask(INV_ALL_SENSORS),
                          mLocalSensorMask(0),
@@ -158,6 +159,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
                          mHaveGoodMpuCal(0),
                          mGyroAccuracy(0),
                          mAccelAccuracy(0),
+                         mCompassAccuracy(0),
                          mSampleCount(0),
                          dmp_orient_fd(-1),
                          mDmpOrientationEnabled(0),
@@ -189,8 +191,8 @@ MPLSensor::MPLSensor(CompassSensor *compass)
 
     pthread_mutex_init(&mMplMutex, NULL);
     pthread_mutex_init(&mHALMutex, NULL);
-    bzero(mGyroOrientation, sizeof(mGyroOrientation));
-    bzero(mAccelOrientation, sizeof(mAccelOrientation));
+    memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
+    memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
 
 #ifdef INV_PLAYBACK_DBG
     LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
@@ -200,10 +202,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
 #endif
 
     /* setup sysfs paths */
-    if(inv_init_sysfs_attributes()) {
-        ALOGE("MPLSensor failed to init sysfs attributes");
-        return;
-    }
+    inv_init_sysfs_attributes();
 
     /* get chip name */
     if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
@@ -236,7 +235,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     }
 
     /* read accel FSR to calcuate accel scale later */
-    if (USE_THIRD_PARTY_ACCEL == 0) {
+    if (!USE_THIRD_PARTY_ACCEL) {
         char buf[3];
         int count = 0;
         LOGV_IF(SYSFS_VERBOSE,
@@ -265,35 +264,26 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
     mPendingEvents[RotationVector].sensor = ID_RV;
     mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
-    mPendingEvents[RotationVector].acceleration.status
-            = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
     mPendingEvents[LinearAccel].sensor = ID_LA;
     mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
-    mPendingEvents[LinearAccel].acceleration.status
-            = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Gravity].version = sizeof(sensors_event_t);
     mPendingEvents[Gravity].sensor = ID_GR;
     mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
-    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Gyro].version = sizeof(sensors_event_t);
     mPendingEvents[Gyro].sensor = ID_GY;
     mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
-    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
     mPendingEvents[RawGyro].sensor = ID_RG;
     mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
-    mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 
     mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
     mPendingEvents[Accelerometer].sensor = ID_A;
     mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
-    mPendingEvents[Accelerometer].acceleration.status
-            = SENSOR_STATUS_ACCURACY_HIGH;
 
     /* Invensense compass calibration */
     mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
@@ -334,6 +324,28 @@ MPLSensor::MPLSensor(CompassSensor *compass)
     else
         LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
 
+    /* Takes external Accel Calibration Load Method */
+    if( m_pt2AccelCalLoadFunc != NULL)
+    {
+        long accel_offset[3];
+        long tmp_offset[3];
+        int result = m_pt2AccelCalLoadFunc(accel_offset);
+        if(result)
+            LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
+        else
+        {
+            LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+            inv_set_accel_bias(accel_offset, mAccelAccuracy);
+            inv_get_accel_bias(tmp_offset, NULL);
+            LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
+               tmp_offset[0], tmp_offset[1], tmp_offset[2]);
+        }
+    }
+    /* End of Accel Calibration Load Method */
+
     inv_set_device_properties();
 
     /* disable driver master enable the first sensor goes on */
@@ -348,36 +360,21 @@ MPLSensor::MPLSensor(CompassSensor *compass)
 
     onPower(0);
 
-    enableDmpOrientation(isDmpDisplayOrientationOn());
-    enableDmpOrientation(!isDmpScreenAutoRotationOn() && isDmpDisplayOrientationOn());
-
-#ifdef INV_PLAYBACK_DBG
-    logfile = fopen("/data/playback.bin", "wb");
-    if (logfile)
-        inv_turn_on_data_logging(logfile);
-#endif
+    if (isDmpDisplayOrientationOn()) {
+        enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
+    }
 
-    mMplSensorInitialized = true;
 }
 
-void MPLSensor::enable_iio_sysfs() {
+void MPLSensor::enable_iio_sysfs()
+{
     VFUNC_LOG;
 
     char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
     FILE *tempFp = NULL;
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
-            mpu.in_timestamp_en, getTimestamp());
-    // Using fopen() here to benefit from fprintf()
-    tempFp = fopen(mpu.in_timestamp_en, "w");
-    if (tempFp == NULL) {
-        LOGE("HAL:could not open timestamp enable");
-    } else {
-        if(fprintf(tempFp, "%d", 1) < 0) {
-            LOGE("HAL:could not enable timestamp");
-        }
-        fclose(tempFp);
-    }
+    /* ignore failures */
+    write_sysfs_int(mpu.in_timestamp_en, 1);
 
     LOGV_IF(SYSFS_VERBOSE,
             "HAL:sysfs:cat %s (%lld)",
@@ -398,30 +395,21 @@ void MPLSensor::enable_iio_sysfs() {
     if (tempFp == NULL) {
         LOGE("HAL:could not open current trigger");
     } else {
-        if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
-            LOGE("HAL:could not write current trigger");
+        if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
+            LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
         }
-        fclose(tempFp);
     }
 
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
-    tempFp = fopen(mpu.buffer_length, "w");
-    if (tempFp == NULL) {
-        LOGE("HAL:could not open buffer length");
-    } else {
-        if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
-            LOGE("HAL:could not write buffer length");
-        }
-        fclose(tempFp);
-    }
+    write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
 
-    inv_get_iio_device_node(iio_device_node);
+    if (inv_get_iio_device_node(iio_device_node) < 0) {
+        LOGE("HAL:could retrive the iio device node");
+    }
     iio_fd = open(iio_device_node, O_RDONLY);
     if (iio_fd < 0) {
         LOGE("HAL:could not open iio device node");
     } else {
-        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd);
+        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
     }
 }
 
@@ -553,9 +541,9 @@ void MPLSensor::inv_set_device_properties()
 
     /* accel setup */
     orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
-    // BMA250
-    //inv_set_accel_orientation_and_scale(orient, 1LL << 22);
-    // MPU6050
+    /* use for third party accel input subsystem driver
+    inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+    */
     inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
 
     /* compass setup */
@@ -573,7 +561,7 @@ void MPLSensor::loadDMP()
     int res, fd;
     FILE *fptr;
 
-    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) {
+    if (isMpu3050()) {
         //DMP support only for MPU6xxx/9xxx currently
         return;
     }
@@ -584,25 +572,26 @@ void MPLSensor::loadDMP()
     fd = open(mpu.firmware_loaded, O_RDONLY);
     if(fd < 0) {
         LOGE("HAL:could not open dmp state");
+        return;
+    }
+    if(inv_read_dmp_state(fd)) {
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
+        return;
+    }
+
+    LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
+    fptr = fopen(mpu.dmp_firmware, "w");
+    if(!fptr) {
+        LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
+        return;
+    }
+    res = inv_load_dmp(fptr);
+    if(res < 0) {
+        LOGE("HAL:load DMP failed");
     } else {
-        if(inv_read_dmp_state(fd) == 0) {
-            LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
-            fptr = fopen(mpu.dmp_firmware, "w");
-            if(!fptr) {
-                LOGE("HAL:could not write to dmp");
-            } else {
-                int res = inv_load_dmp(fptr);
-                if(res < 0) {
-                    LOGE("HAL:load DMP failed");
-                } else {
-                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
-                }
-                fclose(fptr);
-            }
-        } else {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
-        }
+        LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
     }
+    fclose(fptr);
 
     // onDMP(1);                //Can't enable here. See note onDMP()
 }
@@ -693,8 +682,6 @@ MPLSensor::~MPLSensor()
     /* Turn off Gyro master enable          */
     /* A workaround until driver handles it */
     /* TODO: Turn off and close all sensors */
-    LOGV_IF(SYSFS_VERBOSE,
-            "HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp());
     if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
         LOGE("HAL:could not disable gyro master enable");
     }
@@ -741,7 +728,7 @@ int MPLSensor::setGyroInitialState()
     return 0;
 }
 
-/* this applies to BMA250 only */
+/* this applies to BMA250 Input Subsystem Driver only */
 int MPLSensor::setAccelInitialState()
 {
     VFUNC_LOG;
@@ -807,25 +794,22 @@ int MPLSensor::onDMP(int en)
 
     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
             mpu.firmware_loaded, getTimestamp());
-    if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
+    res = read_sysfs_int(mpu.firmware_loaded, &status);
+    if (res < 0){
         LOGE("HAL:ERR can't get firmware_loaded status");
-    } else if (status == 1) {
+        return res;
+    }
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
+
+    if (status) {
         //Write only if curr DMP state <> request
         LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
                 mpu.dmp_on, getTimestamp());
         if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
             LOGE("HAL:ERR can't read DMP state");
         } else if (status != en) {
-            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                    en, mpu.dmp_on, getTimestamp());
-            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
-                LOGE("HAL:ERR can't write dmp_on");
-            } else {
-                res = 0;        //Indicate write successful
-            }
+            res = write_sysfs_int(mpu.dmp_on, en);
             //Enable DMP interrupt
-            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                    en, mpu.dmp_int_on, getTimestamp());
             if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
                 LOGE("HAL:ERR can't en/dis DMP interrupt");
             }
@@ -871,12 +855,7 @@ int MPLSensor::enableQuaternionData(int en)
     VFUNC_LOG;
 
     // Enable DMP quaternion
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.quaternion_on, getTimestamp());
-    if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
-        LOGE("HAL:ERR can't write DMP quaternion_on");
-        res = -1;       //Indicate an err
-    }
+    res = write_sysfs_int(mpu.quaternion_on, en);
 
     if (!en) {
         LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
@@ -884,27 +863,10 @@ int MPLSensor::enableQuaternionData(int en)
 
         LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
     }
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.in_quat_r_en, getTimestamp());
-    if (write_sysfs_int(mpu.in_quat_r_en, en) < 0) {
-        LOGE("HAL:ERR write in_quat_r_en");
-    }
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.in_quat_x_en, getTimestamp());
-    if (write_sysfs_int(mpu.in_quat_x_en, en) < 0) {
-        LOGE("HAL:ERR write in_quat_x_en");
-    }
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.in_quat_y_en, getTimestamp());
-    if (write_sysfs_int(mpu.in_quat_y_en, en) < 0) {
-        LOGE("HAL:ERR write in_quat_y_en");
-    }
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.in_quat_z_en, getTimestamp());
-
-    if (write_sysfs_int(mpu.in_quat_z_en, en) < 0) {
-        LOGE("HAL:ERR write in_quat_z_en");
-    }
+    write_sysfs_int(mpu.in_quat_r_en, en);
+    write_sysfs_int(mpu.in_quat_x_en, en);
+    write_sysfs_int(mpu.in_quat_y_en, en);
+    write_sysfs_int(mpu.in_quat_z_en, en);
 
     LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
 
@@ -939,9 +901,6 @@ int MPLSensor::enablePedometer(int en)
 int MPLSensor::masterEnable(int en)
 {
     VFUNC_LOG;
-
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.chip_enable, getTimestamp());
     return write_sysfs_int(mpu.chip_enable, en);
 }
 
@@ -953,24 +912,14 @@ int MPLSensor::enableGyro(int en)
     int res;
 
     /* need to also turn on/off the master enable */
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.gyro_enable, getTimestamp());
     res = write_sysfs_int(mpu.gyro_enable, en);
 
     if (!en) {
         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
         inv_gyro_was_turned_off();
     } else {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.gyro_x_fifo_enable, getTimestamp());
         write_sysfs_int(mpu.gyro_x_fifo_enable, en);
-
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.gyro_y_fifo_enable, getTimestamp());
         write_sysfs_int(mpu.gyro_y_fifo_enable, en);
-
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.gyro_z_fifo_enable, getTimestamp());
         res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
     }
 
@@ -985,30 +934,17 @@ int MPLSensor::enableAccel(int en)
     int res;
 
     /* need to also turn on/off the master enable */
-    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            en, mpu.accel_enable, getTimestamp());
     res = write_sysfs_int(mpu.accel_enable, en);
 
     if (!en) {
         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
         inv_accel_was_turned_off();
     } else {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.accel_x_fifo_enable, getTimestamp());
         write_sysfs_int(mpu.accel_x_fifo_enable, en);
-
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.accel_y_fifo_enable, getTimestamp());
         write_sysfs_int(mpu.accel_y_fifo_enable, en);
-
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.accel_z_fifo_enable, getTimestamp());
         res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
     }
 
-    if(USE_THIRD_PARTY_ACCEL == 1 && en) {
-        setAccelInitialState(); // BMA250
-    }
     return res;
 }
 
@@ -1017,7 +953,7 @@ int MPLSensor::enableCompass(int en)
     VFUNC_LOG;
 
     int res = mCompassSensor->enable(ID_M, en);
-    if (en == 0) {
+    if (!en) {
         LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
         inv_compass_was_turned_off();
     }
@@ -1069,12 +1005,17 @@ void MPLSensor::computeLocalSensorMask(int enabled_sensors)
     } while (0);
 }
 
+int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
+    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
+    return (this->*enabler)(en);
+}
+
 int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
     VFUNC_LOG;
 
     inv_error_t res = -1;
-    int on = 1;
-    int off = 0;
+    bool store_cal = false;
+    bool ext_compass_changed = false;
 
     // Sequence to enable or disable a sensor
     // 1. enable Power state
@@ -1082,67 +1023,49 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
     // 3. enable or disable a sensor
     // 4. set master enable (=1)
 
-    if (isLowPowerQuatEnabled() ||
-        changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
-        (mCompassSensor->isIntegrated() << MagneticField))) {
+    pthread_mutex_lock(&GlobalHalMutex);
+
+    uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
+            | (1 << MagneticField);
+    uint32_t all_integrated_changeables = all_changeables;
+
+    if (!mCompassSensor->isIntegrated()) {
+        ext_compass_changed = changed & (1 << MagneticField);
+        all_integrated_changeables = all_changeables & ~(1 << MagneticField);
+    }
+
+    if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
         /* ensure power state is on */
         onPower(1);
 
         /* reset master enable */
         res = masterEnable(0);
         if(res < 0) {
-            return res;
+            goto unlock_res;
         }
     }
 
     LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
 
     if (changed & ((1 << Gyro) | (1 << RawGyro))) {
-        if (sensors & INV_THREE_AXIS_GYRO) {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro");
-            res = enableGyro(on);
-            if(res < 0) {
-                return res;
-            }
-        } else if ((sensors & INV_THREE_AXIS_GYRO) == 0) {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
-            res = enableGyro(off);
-            if(res < 0) {
-                return res;
-            }
+        res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
+        if(res < 0) {
+            goto unlock_res;
         }
     }
 
     if (changed & (1 << Accelerometer)) {
-        if (sensors & INV_THREE_AXIS_ACCEL) {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel");
-            res = enableAccel(on);
-            if(res < 0) {
-                return res;
-            }
-        } else if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
-            res = enableAccel(off);
-            if(res < 0) {
-                return res;
-            }
+        res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
+        if(res < 0) {
+            goto unlock_res;
         }
     }
 
     if (changed & (1 << MagneticField)) {
         /* Invensense compass calibration */
-        if (sensors & INV_THREE_AXIS_COMPASS) {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable compass");
-            res = enableCompass(on);
-            if(res < 0) {
-                return res;
-            }
-        } else if ((sensors & INV_THREE_AXIS_COMPASS) == 0) {
-            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable compass");
-            res = enableCompass(off);
-            if(res < 0) {
-                return res;
-            }
+        res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
+        if(res < 0) {
+            goto unlock_res;
         }
     }
 
@@ -1150,14 +1073,13 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
         // Enable LP Quat
         if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
                 (1 << LinearAccel) | (1 << Gravity)))) {
-            if (!(changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
-                    (mCompassSensor->isIntegrated() << MagneticField)))) {
+            if (!(changed & all_integrated_changeables)) {
                 /* ensure power state is on */
                 onPower(1);
                 /* reset master enable */
                 res = masterEnable(0);
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
             }
             if (!checkLPQuaternion()) {
@@ -1170,88 +1092,93 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
         }
     }
 
-    if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
-            (mCompassSensor->isIntegrated() << MagneticField))) {
+    if (changed & all_integrated_changeables) {
         if (sensors &
             (INV_THREE_AXIS_GYRO
                 | INV_THREE_AXIS_ACCEL
                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
-            if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+            if ( isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
                 // disable DMP event interrupt only (w/ data interrupt)
-                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                        0, mpu.dmp_event_int_on, getTimestamp());
                 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
                     res = -1;
                     LOGE("HAL:ERR can't disable DMP event interrupt");
-                    return res;
+                    goto unlock_res;
                 }
             }
 
-            if (isDmpDisplayOrientationOn()) {
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
                 // enable DMP
                 onDMP(1);
 
-                res = enableAccel(on);
+                res = enableAccel(1);
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
 
                 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
                     res = turnOffAccelFifo();
                 }
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
             }
 
             res = masterEnable(1);
             if(res < 0) {
-                return res;
+                goto unlock_res;
             }
         } else { // all sensors idle -> reduce power
-            if (isDmpDisplayOrientationOn()) {
+            if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
                 // enable DMP
                 onDMP(1);
                 // enable DMP event interrupt only (no data interrupt)
-                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                        1, mpu.dmp_event_int_on, getTimestamp());
                 if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
                     res = -1;
                     LOGE("HAL:ERR can't enable DMP event interrupt");
                 }
-                res = enableAccel(on);
+                res = enableAccel(1);
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
                 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
                     res = turnOffAccelFifo();
                 }
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
                 res = masterEnable(1);
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
             }
             else {
                 res = onPower(0);
                 if(res < 0) {
-                    return res;
+                    goto unlock_res;
                 }
             }
-
-            storeCalibration();
+            store_cal = true;
         }
+    } else if (ext_compass_changed &&
+            !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+                | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
+            store_cal = true;
+    }
+
+    if (store_cal || ((changed & all_changeables) != all_changeables)) {
+        storeCalibration();
     }
 
+unlock_res:
+    pthread_mutex_unlock(&GlobalHalMutex);
     return res;
 }
 
 /* Store calibration file */
 void MPLSensor::storeCalibration()
 {
-    if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) {
+    if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
        int res = inv_store_calibration();
        if (res) {
            LOGE("HAL:Cannot store calibration on file");
@@ -1272,8 +1199,9 @@ void MPLSensor::cbProcData()
 int MPLSensor::gyroHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
+    int8_t status;
     int update;
-    update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, &s->timestamp);
+    update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -1282,8 +1210,9 @@ int MPLSensor::gyroHandler(sensors_event_t* s)
 int MPLSensor::rawGyroHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
+    int8_t status;
     int update;
-    update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &s->gyro.status, &s->timestamp);
+    update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -1292,13 +1221,14 @@ int MPLSensor::rawGyroHandler(sensors_event_t* s)
 int MPLSensor::accelHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
+    int8_t status;
     int update;
     update = inv_get_sensor_type_accelerometer(
-        s->acceleration.v, &s->acceleration.status, &s->timestamp);
+        s->acceleration.v, &status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
             s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
             s->timestamp, update);
-    mAccelAccuracy = s->acceleration.status;
+    mAccelAccuracy = status;
     return update;
 }
 
@@ -1310,6 +1240,7 @@ int MPLSensor::compassHandler(sensors_event_t* s)
         s->magnetic.v, &s->magnetic.status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
             s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
+    mCompassAccuracy = s->magnetic.status;
     return update;
 }
 
@@ -1328,9 +1259,10 @@ int MPLSensor::rvHandler(sensors_event_t* s)
 int MPLSensor::laHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
+    int8_t status;
     int update;
     update = inv_get_sensor_type_linear_acceleration(
-            s->gyro.v, &s->gyro.status, &s->timestamp);
+            s->gyro.v, &status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -1339,8 +1271,9 @@ int MPLSensor::laHandler(sensors_event_t* s)
 int MPLSensor::gravHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
+    int8_t status;
     int update;
-    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, &s->timestamp);
+    update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
     LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -1371,7 +1304,12 @@ int MPLSensor::enable(int32_t handle, int en)
             (mDmpOrientationEnabled? "en": "dis"),
             (en? "en" : "dis"));
         enableDmpOrientation(en && isDmpDisplayOrientationOn());
-        mDmpOrientationEnabled = !!en;
+        /* TODO: stop manually testing/using 0 and 1 instead of
+         * false and true, but just use 0 and non-0.
+         * This allows  passing 0 and non-0 ints around instead of
+         * having to convert to 1 and test against 1.
+         */
+        mDmpOrientationEnabled = en;
         return 0;
     case ID_A:
         what = Accelerometer;
@@ -1498,44 +1436,44 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
     int what = -1;
 
     switch (handle) {
-    case ID_SO:
-        return 0;
-    case ID_A:
-        what = Accelerometer;
-        sname = "Accelerometer";
-        break;
-    case ID_M:
-        what = MagneticField;
-        sname = "MagneticField";
-        break;
-    case ID_O:
-        what = Orientation;
-        sname = "Orientation";
-        break;
-    case ID_GY:
-        what = Gyro;
-        sname = "Gyro";
-        break;
-    case ID_RG:
-        what = RawGyro;
-        sname = "RawGyro";
-        break;
-    case ID_GR:
-        what = Gravity;
-        sname = "Gravity";
-        break;
-    case ID_RV:
-        what = RotationVector;
-        sname = "RotationVector";
-        break;
-    case ID_LA:
-        what = LinearAccel;
-        sname = "LinearAccel";
-        break;
-    default: // this takes care of all the gestures
-        what = handle;
-        sname = "Others";
-        break;
+        case ID_SO:
+            return update_delay();
+        case ID_A:
+            what = Accelerometer;
+            sname = "Accelerometer";
+            break;
+        case ID_M:
+            what = MagneticField;
+            sname = "MagneticField";
+            break;
+        case ID_O:
+            what = Orientation;
+            sname = "Orientation";
+            break;
+        case ID_GY:
+            what = Gyro;
+            sname = "Gyro";
+            break;
+        case ID_RG:
+            what = RawGyro;
+            sname = "RawGyro";
+            break;
+        case ID_GR:
+            what = Gravity;
+            sname = "Gravity";
+            break;
+        case ID_RV:
+            what = RotationVector;
+            sname = "RotationVector";
+            break;
+        case ID_LA:
+            what = LinearAccel;
+            sname = "LinearAccel";
+            break;
+        default: // this takes care of all the gestures
+            what = handle;
+            sname = "Others";
+            break;
     }
 
 #if 0
@@ -1627,6 +1565,7 @@ int MPLSensor::update_delay() {
     int res = 0;
     int64_t got;
 
+    pthread_mutex_lock(&GlobalHalMutex);
     if (mEnabled) {
         int64_t wanted = 1000000000;
         int64_t wanted_3rd_party_sensor = 1000000000;
@@ -1679,8 +1618,9 @@ int MPLSensor::update_delay() {
 
         int enabled_sensors = mEnabled;
         int tempFd = -1;
-        if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
-            if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+            if (isLowPowerQuatEnabled() ||
+                    (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
                 bool setDMPrate= 0;
                 // Set LP Quaternion sample rate if enabled
                 if (checkLPQuaternion()) {
@@ -1710,7 +1650,7 @@ int MPLSensor::update_delay() {
             }
 
             //nsToHz (BMA250)
-            if(USE_THIRD_PARTY_ACCEL == 1) {
+            if(USE_THIRD_PARTY_ACCEL) {
                 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
                         wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
                         getTimestamp());
@@ -1726,19 +1666,6 @@ int MPLSensor::update_delay() {
                 inv_set_compass_sample_rate(got / 1000);
             }
 
-            /*
-            //nsTons - nothing to be done
-            strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len],
-                   COMPASS_SENSOR_DELAY);
-            tempFd = open(compass_sensor_sysfs_path, O_RDWR);
-            LOGV_IF(PROCESS_VERBOSE, "setDelay - open path: %s", compass_sensor_sysfs_path);
-            wanted = 20000000LLU;
-            res = write_attribute_sensor(tempFd, wanted);
-            if(res < 0) {
-                LOGE("Compass update delay error");
-            }
-            */
-
         } else {
 
             if (GY_ENABLED) {
@@ -1746,7 +1673,7 @@ int MPLSensor::update_delay() {
                     (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
                     (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
 
-                if (isDmpDisplayOrientationOn()) {
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
                     getDmpRate(&wanted);
                 }
 
@@ -1768,7 +1695,7 @@ int MPLSensor::update_delay() {
                     wanted = mDelays[Accelerometer];
                 }
 
-                if (isDmpDisplayOrientationOn()) {
+                if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
                     getDmpRate(&wanted);
                 }
 
@@ -1776,10 +1703,14 @@ int MPLSensor::update_delay() {
                 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
                         1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
                 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
-                //BMA250 in ms
-                //res = write_attribute_sensor(tempFd, wanted / 1000000L);
-                //MPU6050 in hz
-                res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                if(USE_THIRD_PARTY_ACCEL) {
+                    //BMA250 in ms
+                    res = write_attribute_sensor(tempFd, wanted / 1000000L);
+                }
+                else {
+                    //MPUxxxx in hz
+                    res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
+                }
                 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
             }
 
@@ -1799,7 +1730,7 @@ int MPLSensor::update_delay() {
                         wanted = mDelays[MagneticField];
                     }
 
-                    if (isDmpDisplayOrientationOn()) {
+                    if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
                         getDmpRate(&wanted);
                     }
                 }
@@ -1817,21 +1748,15 @@ int MPLSensor::update_delay() {
                 | INV_THREE_AXIS_ACCEL
                 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
             res = masterEnable(1);
-            if(res < 0) {
-                return res;
-            }
         } else { // all sensors idle -> reduce power
             res = onPower(0);
-            if(res < 0) {
-                return res;
-            }
         }
     }
-
+    pthread_mutex_unlock(&GlobalHalMutex);
     return res;
 }
 
-/* use for third party accel */
+/* For Third Party Accel Input Subsystem Drivers only */
 /* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
 int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
 {
@@ -1850,7 +1775,7 @@ int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
     input_event const* event;
     int nb, done = 0;
 
-    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
+    while (!done && count && mAccelInputReader.readEvent(&event)) {
         int type = event->type;
         if (type == EV_ABS) {
             if (event->code == EVENT_TYPE_ACCEL_X) {
@@ -1937,7 +1862,7 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count)
 int MPLSensor::readEvents(sensors_event_t *data, int count) {
 
 
-    int lp_quaternion_on, nbyte;
+    int lp_quaternion_on = 0, nbyte;
     int i, nb, mask = 0, numEventReceived = 0,
         sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
@@ -1948,7 +1873,7 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
 
     if (isLowPowerQuatEnabled()) {
         lp_quaternion_on = checkLPQuaternion();
-        if (lp_quaternion_on == 1) {
+        if (lp_quaternion_on) {
             nbyte += sizeof(mCachedQuaternionData);             //currently 16 bytes for Q data
         }
     }
@@ -1990,7 +1915,7 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
         return -1;
     }
 
-    if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
+    if (isLowPowerQuatEnabled() && lp_quaternion_on) {
 
         for (i=0; i< 4; i++) {
             mCachedQuaternionData[i]= *(long*)rdata;
@@ -2092,7 +2017,7 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
         }
     }
 
-    if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
+    if (isLowPowerQuatEnabled() && lp_quaternion_on) {
 
         inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
         LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
@@ -2122,8 +2047,8 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
     // pthread_mutex_lock(&mHALMutex);
 
     done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
-#ifdef COMPASS_YAS530
-    if (mCompassSensor->checkCoilsReset()==1) {
+#ifdef COMPASS_YAS53x
+    if (mCompassSensor->checkCoilsReset()) {
        //Reset relevant compass settings
        resetCompass();
     }
@@ -2149,7 +2074,7 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
     return numEventReceived;
 }
 
-#ifdef COMPASS_YAS530
+#ifdef COMPASS_YAS53x
 int MPLSensor::resetCompass()
 {
     VFUNC_LOG;
@@ -2198,8 +2123,6 @@ int MPLSensor::turnOffAccelFifo() {
         mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
 
     for (i = 0; i < 3; i++) {
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-            0, accel_fifo_enable[i], getTimestamp());
         res = write_sysfs_int(accel_fifo_enable[i], 0);
         if (res < 0) {
             return res;
@@ -2213,57 +2136,63 @@ int MPLSensor::enableDmpOrientation(int en)
     VFUNC_LOG;
     /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
     int res = 0;
+    int enabled_sensors = mEnabled;
+
+    if (isMpu3050())
+        return res;
+
+    pthread_mutex_lock(&GlobalHalMutex);
 
     // on power if not already On
-    onPower(1);
+    res = onPower(1);
     // reset master enable
-    masterEnable(0);
+    res = masterEnable(0);
 
-    if (en == 1) {
+    if (en) {
         //Enable DMP orientation
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                en, mpu.display_orientation_on, getTimestamp());
         if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
             LOGE("HAL:ERR can't enable Android orientation");
-            res = -1;    //Indicate an err
+            res = -1;	// indicate an err
         }
 
-        //Open DMP Orient Fd
-        openDmpOrientFd();
+        // open DMP Orient Fd
+        res = openDmpOrientFd();
 
-        //Enable DMP
-        onDMP(1);
+        // enable DMP
+        res = onDMP(1);
 
-        //Default DMP output rate to FIFO
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                5, mpu.dmp_output_rate, getTimestamp());
+        // default DMP output rate to FIFO
         if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
             LOGE("HAL:ERR can't default DMP output rate");
         }
 
-        //Set DMP rate to 200Hz
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                200, mpu.accel_fifo_rate, getTimestamp());
+        // set DMP rate to 200Hz
         if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
             res = -1;
             LOGE("HAL:ERR can't set DMP rate to 200Hz");
         }
 
         // enable accel engine
-        enableAccel(1);
+        res = enableAccel(1);
 
         // disable accel FIFO
-        res = turnOffAccelFifo();
+        if (!A_ENABLED) {
+            res = turnOffAccelFifo();
+        }
 
-        mFeatureActiveMask |=INV_DMP_DISPL_ORIENTATION;
+        mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
     } else {
         // disable DMP
-        onDMP(0);
+        res = onDMP(0);
+
         // disable accel engine
-        enableAccel(0);
+        if (!A_ENABLED) {
+            res = enableAccel(0);
+        }
     }
 
     res = masterEnable(1);
+    pthread_mutex_unlock(&GlobalHalMutex);
     return res;
 }
 
@@ -2364,9 +2293,6 @@ int MPLSensor::getDmpRate(int64_t *wanted)
 {
     if (checkDMPOrientation() || checkLPQuaternion()) {
         // set DMP output rate to FIFO
-        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
-                int(1000000000.f / *wanted), mpu.dmp_output_rate,
-                getTimestamp());
         write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
         LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
 
@@ -2749,34 +2675,39 @@ int MPLSensor::inv_init_sysfs_attributes(void)
 {
     VFUNC_LOG;
 
-    unsigned char i = 0;
-    char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2];
+    unsigned char i;
+    char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
     char *sptr;
     char **dptr;
     int num;
 
+    sysfs_names_ptr =
+            (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr == NULL) {
+        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    dptr = (char**)&mpu;
+    i = 0;
+    do {
+        *dptr++ = sptr;
+        sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+    } while (++i < MAX_SYSFS_ATTRB);
+
     // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
     // inv_get_sysfs_abs_path(sysfs_path);
     if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
         ALOGE("MPLSensor failed get sysfs path");
         return -1;
     }
-    sysfs_names_ptr =
-            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
-    sptr = sysfs_names_ptr;
-    if (sptr != NULL) {
-        dptr = (char**)&mpu;
-        do {
-            *dptr++ = sptr;
-            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
-        } while (++i < MAX_SYSFS_ATTRB);
-    } else {
-        LOGE("HAL:couldn't alloc mem for sysfs paths");
+
+    if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
+        ALOGE("MPLSensor failed get iio trigger path");
         return -1;
     }
 
-    inv_get_iio_trigger_path(iio_trigger_path);
-
     sprintf(mpu.key, "%s%s", sysfs_path, "/key");
     sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
     sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
@@ -2804,22 +2735,15 @@ int MPLSensor::inv_init_sysfs_attributes(void)
     sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
     sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
 
-#ifdef THIRD_PARTY_ACCEL    //BMA250
-    /* same as 'mpu.accel_enable' */
     sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
     sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
-    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA");
-    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA");
     sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
-#else
-    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
-    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
-    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
 
+
+#ifndef THIRD_PARTY_ACCEL //MPUxxxx
+    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
     // TODO: for bias settings
     sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
-
-    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
 #endif
 
     sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
@@ -2844,3 +2768,31 @@ int MPLSensor::inv_init_sysfs_attributes(void)
 #endif
     return 0;
 }
+
+/* TODO: stop manually testing/using 0 and 1 instead of
+ * false and true, but just use 0 and non-0.
+ * This allows  passing 0 and non-0 ints around instead of
+ * having to convert to 1 and test against 1.
+ */
+bool MPLSensor::isMpu3050()
+{
+    return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
+}
+
+int MPLSensor::isLowPowerQuatEnabled()
+{
+#ifdef ENABLE_LP_QUAT_FEAT
+    return !isMpu3050();
+#else
+    return 0;
+#endif
+}
+
+int MPLSensor::isDmpDisplayOrientationOn()
+{
+#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
+    return !isMpu3050();
+#else
+    return 0;
+#endif
+}
diff --git a/libsensors_iio/MPLSensor.h b/libsensors_iio/MPLSensor.h
index 879f37b..4162324 100644
--- a/libsensors_iio/MPLSensor.h
+++ b/libsensors_iio/MPLSensor.h
@@ -30,8 +30,8 @@
 
 #ifdef INVENSENSE_COMPASS_CAL
 
-#ifdef COMPASS_YAS530
-#warning "unified HAL for YAS530"
+#ifdef COMPASS_YAS53x
+#warning "unified HAL for YAS53x"
 #include "CompassSensor.IIO.primary.h"
 #elif COMPASS_AMI306
 #warning "unified HAL for AMI306"
@@ -49,7 +49,6 @@
 /* Sensors Enable/Disable Mask
  *****************************************************************************/
 #define MAX_CHIP_ID_LEN             (20)
-#define IIO_BUFFER_LENGTH           (100 * 2)
 
 #define INV_THREE_AXIS_GYRO         (0x000F)
 #define INV_THREE_AXIS_ACCEL        (0x0070)
@@ -72,31 +71,32 @@
 #define INV_DMP_QUATERNION           0x04
 #define INV_DMP_DISPL_ORIENTATION    0x08
 
-// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */
-// #define ENABLE_DMP_DISPL_ORIENT_FEAT /* Uncomment to enable DMP display orientation */
+/* Uncomment to enable Low Power Quaternion */
+#define ENABLE_LP_QUAT_FEAT
 
-#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
-/* Uncomment followings to enable screen auto-rotation by DMP (need the latest Android source tree update) */
-// #define ENABLE_DMP_SCREEN_AUTO_ROTATION
-#endif
-
-int isLowPowerQuatEnabled() {
-#ifdef ENABLE_LP_QUAT_FEAT
-    return 1;
-#else
-    return 0;
-#endif
-}
+/* Uncomment to enable DMP display orientation 
+   (within the HAL, see below for Java framework) */
+// #define ENABLE_DMP_DISPL_ORIENT_FEAT
 
-int isDmpDisplayOrientationOn() {
 #ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
-    return 1;
-#else
-    return 0;
+/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION 
+   sensor type (DMP screen orientation) to the Java framework.
+   NOTE:
+       need Invensense customized 
+       'hardware/libhardware/include/hardware/sensors.h' to compile correctly.
+   NOTE:
+       need Invensense java framework changes to:
+       'frameworks/base/core/java/android/view/WindowOrientationListener.java'
+       'frameworks/base/core/java/android/hardware/Sensor.java'
+       'frameworks/base/core/java/android/hardware/SensorEvent.java'
+       for the 'Auto-rotate screen' to use this feature.
+*/
+#define ENABLE_DMP_SCREEN_AUTO_ROTATION
+#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly"
 #endif
-}
 
-int isDmpScreenAutoRotationOn() {
+int isDmpScreenAutoRotationEnabled()
+{
 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
     return 1;
 #else
@@ -104,17 +104,19 @@ int isDmpScreenAutoRotationOn() {
 #endif
 }
 
+int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;
 /*****************************************************************************/
 /** MPLSensor implementation which fits into the HAL example for crespo provided
  *  by Google.
  *  WARNING: there may only be one instance of MPLSensor, ever.
  */
- 
+
 class MPLSensor: public SensorBase
 {
     typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
 
 public:
+
     enum {
         Gyro = 0,
         RawGyro,
@@ -127,7 +129,7 @@ public:
         numSensors
     };
 
-    MPLSensor(CompassSensor *);
+    MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
     virtual ~MPLSensor();
 
     virtual int setDelay(int32_t handle, int64_t ns);
@@ -145,9 +147,6 @@ public:
     int populateSensorList(struct sensor_t *list, int len);
     void cbProcData();
 
-    // Do not work with this object unless it is initialized
-    bool isValid() { return mMplSensorInitialized; };
-
     //static pointer to the object that will handle callbacks
     static MPLSensor* gMPLSensor;
 
@@ -169,9 +168,6 @@ public:
     int checkDMPOrientation();
 
 protected:
-    // Lets us know if the constructor was actually able to finish its job.
-    // E.g. false if init sysfs failed.
-    bool mMplSensorInitialized;
     CompassSensor *mCompassSensor;
 
     int gyroHandler(sensors_event_t *data);
@@ -195,6 +191,7 @@ protected:
     int enableLPQuaternion(int);
     int enableQuaternionData(int);
     int onDMP(int);
+    int enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int));
     int enableGyro(int en);
     int enableAccel(int en);
     int enableCompass(int en);
@@ -210,7 +207,7 @@ protected:
     int inv_read_sensor_bias(int fd, long *data);
     void inv_get_sensors_orientation(void);
     int inv_init_sysfs_attributes(void);
-#ifdef COMPASS_YAS530
+#ifdef COMPASS_YAS53x
     int resetCompass(void);
 #endif
     void setCompassDelay(int64_t ns);
@@ -228,6 +225,7 @@ protected:
     bool mHaveGoodMpuCal;   // flag indicating that the cal file can be written
     int mGyroAccuracy;      // value indicating the quality of the gyro calibr.
     int mAccelAccuracy;     // value indicating the quality of the accel calibr.
+    int mCompassAccuracy;     // value indicating the quality of the compass calibr.
     struct pollfd mPollFds[5];
     int mSampleCount;
     pthread_mutex_t mMplMutex;
@@ -243,6 +241,7 @@ protected:
 
     int mDmpOrientationEnabled;
 
+
     uint32_t mEnabled;
     uint32_t mOldEnabledMask;
     sensors_event_t mPendingEvents[numSensors];
@@ -332,6 +331,11 @@ private:
     void fillLinearAccel(struct sensor_t *list);
     void storeCalibration();
     void loadDMP();
+    bool isMpu3050();
+    int isLowPowerQuatEnabled();
+    int isDmpDisplayOrientationOn();
+
+
 };
 
 extern "C" {
diff --git a/libsensors_iio/MPLSupport.cpp b/libsensors_iio/MPLSupport.cpp
index dcb12d2..9773562 100644
--- a/libsensors_iio/MPLSupport.cpp
+++ b/libsensors_iio/MPLSupport.cpp
@@ -14,12 +14,30 @@
 * limitations under the License.
 */
 
+#define LOG_NDEBUG 0
+
 #include <MPLSupport.h>
 #include <string.h>
 #include <stdio.h>
+#include <fcntl.h>
+
 #include "log.h"
 #include "SensorBase.h"
-#include <fcntl.h>
+
+#include "ml_sysfs_helper.h"
+#include "local_log_def.h"
+
+int64_t getTimestamp()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+}
+
+int64_t timevalToNano(timeval const& t) {
+    return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
+}
 
 int inv_read_data(char *fname, long *data)
 {
@@ -138,16 +156,35 @@ int read_sysfs_int(char *filename, int *var)
 
 int write_sysfs_int(char *filename, int var)
 {
-    int res=0;
+    int res = 0;
     FILE  *sysfsfp;
 
+    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+            var, filename, getTimestamp());
     sysfsfp = fopen(filename, "w");
-    if (sysfsfp!=NULL) {
-        fprintf(sysfsfp, "%d\n", var);
-        fclose(sysfsfp);
-    } else {
+    if (sysfsfp == NULL) {
         res = -errno;
-        LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+        LOGE("HAL:ERR open file %s to write with error %d", filename, res);
+        return res;
+    }
+    int fpres, fcres = 0;
+    fpres = fprintf(sysfsfp, "%d\n", var);
+    /* fprintf() can succeed because it never actually writes to the
+     * underlying sysfs node.
+     */
+    if (fpres < 0) {
+       res = -errno;
+       fclose(sysfsfp);
+    } else {
+        fcres = fclose(sysfsfp);
+        /* Check for errors from: fflush(), write(), and close() */
+        if (fcres < 0) {
+            res = -errno;
+        }
+    }
+    if (fpres < 0 || fcres < 0) {
+        LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d",
+            var, filename, res, fpres, fcres);
     }
     return res;
 }
diff --git a/libsensors_iio/MPLSupport.h b/libsensors_iio/MPLSupport.h
index 73604ba..98e4497 100644
--- a/libsensors_iio/MPLSupport.h
+++ b/libsensors_iio/MPLSupport.h
@@ -18,12 +18,16 @@
 #define ANDROID_MPL_SUPPORT_H
 
 #include <stdint.h>
+#include <time.h>
 
-    int inv_read_data(char *fname, long *data);
-    int read_attribute_sensor(int fd, char* data, unsigned int size);
-    int enable_sysfs_sensor(int fd, int en);
-    int write_attribute_sensor(int fd, long data);
-    int read_sysfs_int(char*, int*);
-    int write_sysfs_int(char*, int);
+int64_t getTimestamp();
+int64_t timevalToNano(timeval const& t);
+
+int inv_read_data(char *fname, long *data);
+int read_attribute_sensor(int fd, char* data, unsigned int size);
+int enable_sysfs_sensor(int fd, int en);
+int write_attribute_sensor(int fd, long data);
+int read_sysfs_int(char*, int*);
+int write_sysfs_int(char*, int);
 
 #endif //  ANDROID_MPL_SUPPORT_H
diff --git a/libsensors_iio/SensorBase.cpp b/libsensors_iio/SensorBase.cpp
index fd0e2ca..21d0ed0 100644
--- a/libsensors_iio/SensorBase.cpp
+++ b/libsensors_iio/SensorBase.cpp
@@ -18,7 +18,6 @@
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
-#include <unistd.h>
 #include <dirent.h>
 #include <sys/select.h>
 #include <cutils/log.h>
@@ -86,14 +85,6 @@ bool SensorBase::hasPendingEvents() const
     return false;
 }
 
-int64_t SensorBase::getTimestamp()
-{
-    struct timespec t;
-    t.tv_sec = t.tv_nsec = 0;
-    clock_gettime(CLOCK_MONOTONIC, &t);
-    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
-}
-
 int SensorBase::openInput(const char *inputName)
 {
     int fd = -1;
diff --git a/libsensors_iio/SensorBase.h b/libsensors_iio/SensorBase.h
index d9abe92..fef47c7 100644
--- a/libsensors_iio/SensorBase.h
+++ b/libsensors_iio/SensorBase.h
@@ -25,6 +25,7 @@
 #define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
 
 #define MAX_SYSFS_NAME_LEN  (100)
+#define IIO_BUFFER_LENGTH   (480)
 
 /*****************************************************************************/
 
@@ -39,10 +40,6 @@ protected:
     int data_fd;
 
     int openInput(const char* inputName);
-    static int64_t getTimestamp();
-    static int64_t timevalToNano(timeval const& t) {
-        return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
-    }
 
     int open_device();
     int close_device();
diff --git a/libsensors_iio/libmllite.so b/libsensors_iio/libmllite.so
index 98147a2..9bdd5bc 100644..100755
--- a/libsensors_iio/libmllite.so
+++ b/libsensors_iio/libmllite.so
diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so
index fbd346f..205ab9a 100644
--- a/libsensors_iio/libmplmpu.so
+++ b/libsensors_iio/libmplmpu.so
diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h
index 4127dd7..9135992 100644
--- a/libsensors_iio/local_log_def.h
+++ b/libsensors_iio/local_log_def.h
@@ -1,9 +1,6 @@
 #ifndef LOCAL_LOG_DEF_H
 #define LOCAL_LOG_DEF_H
 
-/* comment this line if Android OS is ICS and prior */
-#define ANDROID_VERSION_JB      (1)
-
 /* Log enablers, each of these independent */
 
 #define PROCESS_VERBOSE (0) /* process log messages */
@@ -18,7 +15,7 @@
 #define INPUT_DATA      (0) /* log the data input from the events */
 #define HANDLER_DATA    (0) /* log the data fetched from the handlers */
 
-#ifdef ANDROID_VERSION_JB
+#if defined ANDROID_JELLYBEAN
 #define LOGV            ALOGV
 #define LOGV_IF         ALOGV_IF
 #define LOGD            ALOGD
@@ -37,8 +34,11 @@
 #define LOG_ASSERT      ALOG_ASSERT
 #define LOG                     ALOG
 #define IF_LOG          IF_ALOG
+#else
+#warning "build for ICS or earlier version"
 #endif
 
+
 #define FUNC_LOG \
             LOGV("%s", __PRETTY_FUNCTION__)
 #define VFUNC_LOG \
@@ -46,4 +46,4 @@
 #define VHANDLER_LOG \
             LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
 
-#endif
+#endif /*ifndef LOCAL_LOG_DEF_H */
diff --git a/libsensors_iio/sensor_params.h b/libsensors_iio/sensor_params.h
index c51d87a..39e3e5c 100644
--- a/libsensors_iio/sensor_params.h
+++ b/libsensors_iio/sensor_params.h
@@ -62,11 +62,11 @@
 #define COMPASS_YAS529_RANGE            (19660.f)
 #define COMPASS_YAS529_RESOLUTION       (0.012f)
 #define COMPASS_YAS529_POWER            (4.f)
-//COMPASS_ID_YAS530
-#define COMPASS_YAS530_RANGE            (8001.f)
-#define COMPASS_YAS530_RESOLUTION       (0.012f)
-#define COMPASS_YAS530_POWER            (4.f)
-#define COMPASS_YAS530_MINDELAY         (10000)
+//COMPASS_ID_YAS53x
+#define COMPASS_YAS53x_RANGE            (8001.f)
+#define COMPASS_YAS53x_RESOLUTION       (0.012f)
+#define COMPASS_YAS53x_POWER            (4.f)
+#define COMPASS_YAS53x_MINDELAY         (10000)
 //COMPASS_ID_HMC5883
 #define COMPASS_HMC5883_RANGE           (10673.f)
 #define COMPASS_HMC5883_RESOLUTION      (10.f)
diff --git a/libsensors_iio/sensors.h b/libsensors_iio/sensors.h
index 7264043..f698fc5 100644
--- a/libsensors_iio/sensors.h
+++ b/libsensors_iio/sensors.h
@@ -35,10 +35,8 @@ __BEGIN_DECLS
 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
 #endif
 
-#define ID_MPL_BASE (0)
-
 enum {
-    ID_GY = ID_MPL_BASE,
+    ID_GY = 0,
     ID_RG,
     ID_A,
     ID_M,
@@ -49,16 +47,6 @@ enum {
     ID_SO
 };
 
-/*
-#define ID_GY (ID_MPL_BASE)
-#define ID_A  (ID_GY + 1)
-#define ID_M  (ID_A + 1)
-#define ID_O  (ID_M + 1)
-#define ID_RV (ID_O + 1)
-#define ID_LA (ID_RV + 1)
-#define ID_GR (ID_LA + 1)
-*/
-
 /*****************************************************************************/
 
 /*
diff --git a/libsensors_iio/sensors_mpl.cpp b/libsensors_iio/sensors_mpl.cpp
index 4445309..4aef514 100644
--- a/libsensors_iio/sensors_mpl.cpp
+++ b/libsensors_iio/sensors_mpl.cpp
@@ -32,20 +32,25 @@
 
 #include "sensors.h"
 #include "MPLSensor.h"
-#include "local_log_def.h"
 
 /*****************************************************************************/
 /* The SENSORS Module */
 
 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
-#define LOCAL_SENSORS (numSensors + 1)
+#define LOCAL_SENSORS (MPLSensor::numSensors + 1)
 #else
-#define LOCAL_SENSORS numSensors
-
+#define LOCAL_SENSORS MPLSensor::numSensors
 #endif
 
+/* Vendor-defined Accel Load Calibration File Method
+* @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame
+* @return '0' for a successful load, '1' otherwise
+* example: int AccelLoadConfig(long* offset);
+* End of Vendor-defined Accel Load Cal Method
+*/
+
 static struct sensor_t sSensorList[LOCAL_SENSORS];
-static int sensors = LOCAL_SENSORS;
+static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
 
 static int open_sensors(const struct hw_module_t* module, const char* id,
                         struct hw_device_t** device);
@@ -101,9 +106,15 @@ private:
 sensors_poll_context_t::sensors_poll_context_t() {
     VFUNC_LOG;
 
-    CompassSensor *mCompassSensor = new CompassSensor();
+    mCompassSensor = new CompassSensor();
     MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
 
+   /* For Vendor-defined Accel Calibration File Load
+    * Use the Following Constructor and Pass Your Load Cal File Function
+    *
+    * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
+    */
+
     // setup the callback object for handing mpl callbacks
     setCallbackObject(mplSensor);
 
@@ -156,11 +167,13 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
             if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
                 nb = 0;
                 if (i == mpl) {
-                    nb = mSensor->readEvents(data, count);
+                    /* Ignore res */
+                    mSensor->readEvents(NULL, 0);
                     mPollFds[i].revents = 0;
                 }
                 else if (i == compass) {
-                    nb = ((MPLSensor*) mSensor)->readCompassEvents(data, count);
+                    /* Ignore res */
+                    ((MPLSensor*) mSensor)->readCompassEvents(NULL, count);
                     mPollFds[i].revents = 0;
                 }
             }
@@ -175,7 +188,7 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
         if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {
             nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);
             mPollFds[dmpOrient].revents= 0;
-            if (isDmpScreenAutoRotationOn() && nb > 0) {
+            if (isDmpScreenAutoRotationEnabled() && nb > 0) {
                 count -= nb;
                 nbEvents += nb;
                 data += nb;
diff --git a/libsensors_iio/software/build/android/common.mk b/libsensors_iio/software/build/android/common.mk
index b84a99c..84e7e9b 100644
--- a/libsensors_iio/software/build/android/common.mk
+++ b/libsensors_iio/software/build/android/common.mk
@@ -4,6 +4,9 @@ SHELL = /bin/bash
 ####################################################################################################
 ## defines
 
+# Build for Jellybean 
+BUILD_ANDROID_JELLYBEAN = 1
+
 ## libraries ##
 LIB_PREFIX = lib
 
@@ -16,23 +19,39 @@ TARGET ?= android
 
 MLLITE_LIB_NAME     ?= mllite
 MPL_LIB_NAME        ?= mplmpu
-HALWRAPPER_LIB_NAME ?= androidhal
 
 ## applications ##
 SHARED_APP_SUFFIX = -shared
 STATIC_APP_SUFFIX = -static
 
 ####################################################################################################
-## includes and linker
+## compile, includes, and linker
+
+ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
+endif
 
-ANDROID_LINK  = -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+ANDROID_LINK  = -nostdlib
+ANDROID_LINK += -fpic
+ANDROID_LINK += -Wl,--gc-sections 
+ANDROID_LINK += -Wl,--no-whole-archive 
 ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib 
+ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+ANDROID_LINK_EXECUTABLE  = $(ANDROID_LINK)
+ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker
+ifneq ($(BUILD_ANDROID_JELLYBEAN),1)
+ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
+endif
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
+ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
 
 ANDROID_INCLUDES  = -I$(ANDROID_ROOT)/system/core/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include   # ICS
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
 ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
diff --git a/libsensors_iio/software/build/android/shared.mk b/libsensors_iio/software/build/android/shared.mk
index f3a123f..47dedfe 100644
--- a/libsensors_iio/software/build/android/shared.mk
+++ b/libsensors_iio/software/build/android/shared.mk
@@ -24,6 +24,7 @@ ifeq ($(BUILD_MPL),1)
 endif
 APP_FOLDERS  = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
 APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET)
 
 INSTALL_DIR = $(CURDIR)
 
diff --git a/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h
index 0a747ce..c519691 100644
--- a/libsensors_iio/software/core/driver/include/log.h
+++ b/libsensors_iio/software/core/driver/include/log.h
@@ -37,8 +37,8 @@
 #ifndef _LIBS_CUTILS_MPL_LOG_H
 #define _LIBS_CUTILS_MPL_LOG_H
 
+#include <stdlib.h>
 #include <stdarg.h>
-#include "local_log_def.h"
 
 #ifdef ANDROID
 #ifdef NDK_BUILD
@@ -56,6 +56,7 @@
 extern "C" {
 #endif
 
+
 /* --------------------------------------------------------------------- */
 
 /*
@@ -287,7 +288,7 @@ extern "C" {
 #ifndef MPL_LOG_PRI
 #ifdef ANDROID
 #define MPL_LOG_PRI(priority, tag, fmt, ...) \
-	LOG(priority, tag, fmt, ##__VA_ARGS__)
+	ALOG(priority, tag, fmt, ##__VA_ARGS__)
 #elif defined __KERNEL__
 #define MPL_LOG_PRI(priority, tag, fmt, ...) \
 	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
diff --git a/libsensors_iio/software/core/mllite/build/android/libmllite.so b/libsensors_iio/software/core/mllite/build/android/libmllite.so
index 98147a2..9bdd5bc 100644..100755
--- a/libsensors_iio/software/core/mllite/build/android/libmllite.so
+++ b/libsensors_iio/software/core/mllite/build/android/libmllite.so
diff --git a/libsensors_iio/software/core/mllite/build/android/shared.mk b/libsensors_iio/software/core/mllite/build/android/shared.mk
index 2ee2e20..1418450 100644
--- a/libsensors_iio/software/core/mllite/build/android/shared.mk
+++ b/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -15,6 +15,7 @@ MLLITE_DIR  = $(INV_ROOT)/software/core/mllite
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -44,12 +45,7 @@ LLINK += -ldl
 LFLAGS += $(CMDLINE_LFLAGS)
 LFLAGS += -shared 
 LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -nostdlib 
-LFLAGS += -fpic 
-LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc 
-LFLAGS += -Wl,--gc-sections 
-LFLAGS += -Wl,--no-whole-archive 
-LFLAGS += -Wl,-shared,-Bsymbolic 
+LFLAGS += -Wl,-shared,-Bsymbolic
 LFLAGS += $(ANDROID_LINK)
 LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c
index b139771..48993bc 100644
--- a/libsensors_iio/software/core/mllite/data_builder.c
+++ b/libsensors_iio/software/core/mllite/data_builder.c
@@ -18,6 +18,8 @@
 #undef MPL_LOG_NDEBUG
 #define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
 
+#include <string.h>
+
 #include "ml_math_func.h"
 #include "data_builder.h"
 #include "mlmath.h"
@@ -429,7 +431,7 @@ void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
     raw32[1] = (long)sensor->raw[1] << 15;
     raw32[2] = (long)sensor->raw[2] << 15;
 
-    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_data);
+    inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
 
     raw32[0] -= bias[0] >> 1;
     raw32[1] -= bias[1] >> 1;
@@ -490,6 +492,16 @@ void inv_set_accel_bias(const long *bias, int accuracy)
     inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
 }
 
+/** Sets the accel accuracy.
+* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
+*/
+void inv_set_accel_accuracy(int accuracy)
+{
+    sensors.accel.accuracy = accuracy;
+    inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
+}
+
 /** Sets the accel bias with control over which axis.
 * @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
 * @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
@@ -512,6 +524,7 @@ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
     }
     sensors.accel.accuracy = accuracy;
     inv_data_builder.save.accel_accuracy = accuracy;
+    inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
 }
 
 
@@ -1010,7 +1023,7 @@ void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
 */
 void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
 {
-    memcpy(data, sensors.gyro.raw_data, sizeof(sensors.gyro.raw_data));
+    memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
     if (timestamp != NULL) {
         *timestamp = sensors.gyro.timestamp;
     }
diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h
index 4263922..9aa46e6 100644
--- a/libsensors_iio/software/core/mllite/data_builder.h
+++ b/libsensors_iio/software/core/mllite/data_builder.h
@@ -71,7 +71,7 @@ struct inv_single_sensor_t {
     /** The raw data in raw data units in the mounting frame */
     short raw[3];
     /** Raw data in body frame */
-    long raw_data[3];
+    long raw_scaled[3];
     /** Calibrated data */
     long calibrated[3];
     long sensitivity;
@@ -188,6 +188,7 @@ void inv_set_compass_bias(const long *bias, int accuracy);
 void inv_set_compass_disturbance(int dist);
 void inv_set_gyro_bias(const long *bias, int accuracy);
 void inv_set_accel_bias(const long *bias, int accuracy);
+void inv_set_accel_accuracy(int accuracy);
 void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask);
 
 void inv_get_gyro_bias(long *bias, long *temp);
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c
index 5e7b2cc..7cdca59 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.c
+++ b/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -11,9 +11,12 @@
  *              Sets up common outputs for HAL
  *
  *   @{
- *       @file hal_outputs.c
+ *       @file  hal_outputs.c
  *       @brief HAL Outputs.
  */
+
+#include <string.h>
+
 #include "hal_outputs.h"
 #include "log.h"
 #include "ml_math_func.h"
@@ -121,6 +124,10 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
     return status;
 }
 
+/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
+ * So this is: pi / 2^16 / 180 */
+#define GYRO_CONVERSION 2.66316109007924e-007f
+
 /** Gyroscope calibrated data (rad/s) in body frame.
 * @param[out] values Rotation Rate in rad/sec.
 * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
@@ -131,9 +138,6 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
 int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
                                    inv_time_t * timestamp)
 {
-    /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
-     * So this is: pi / 2^16 / 180 */
-#define GYRO_CONVERSION 2.66316109007924e-007f
     long gyro[3];
     int status;
 
@@ -158,9 +162,6 @@ int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
 int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
                                    inv_time_t * timestamp)
 {
-    /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
-     * So this is: pi / 2^16 / 180 */
-#define GYRO_CONVERSION 2.66316109007924e-007f
     long gyro[3];
     int status;
 
@@ -353,8 +354,19 @@ inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
         use_sensor = 3;
     }
 
+    // Only output 9-axis if all 9 sensors are on.
+    if (sensor_cal->quat.status & INV_SENSOR_ON) {
+        // If quaternion sensor is on, gyros are not required as quaternion already has that part
+        if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+            use_sensor = -1;
+        }
+    } else {
+        if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+            use_sensor = -1;
+        }
+    }
+
     switch (use_sensor) {
-    default:
     case 0:
         hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
         hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
@@ -371,6 +383,9 @@ inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
         hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
         hal_out.nav_timestamp = sensor_cal->quat.timestamp;
         break;
+    default:
+        hal_out.nine_axis_status = 0; // Don't output quaternion related info
+        break;
     }
 
     /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
index 91c766b..72940f7 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -19,11 +19,11 @@
  */
 #include <stdio.h>
 
+#include "log.h"
 #undef MPL_LOG_TAG
 #define MPL_LOG_TAG "MPL-loaddmp"
 
 #include "ml_load_dmp.h"
-#include "log.h"
 #include "mlos.h"
 
 #define LOADDMP_LOG MPL_LOGI
@@ -69,15 +69,15 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 
     /* bank # 2 */
     0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0x00, 0x01, 0x00, 0x44, 0x00, 0x05, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 
     0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 
     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
-    0xc0, 0x00, 0x00, 0x01, 0x3f, 0xff, 0xff, 0xff, 0x40, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, 
     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 
@@ -106,9 +106,9 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
     0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 
     0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 
     0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, 
-    0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, 0xca, 0xf1, 
-    0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, 0x99, 0x2c, 
-    0x54, 0x7c, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 
+    0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, 
+    0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 
+    0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 
     0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 
     0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 
     0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, 
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
index 24b3217..b4c4249 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -21,13 +21,12 @@
 
 #include <stdio.h>
 
+#include "log.h"
 #undef MPL_LOG_TAG
 #define MPL_LOG_TAG "MPL-storeload"
 
-
 #include "ml_stored_data.h"
 #include "storage_manager.h"
-#include "log.h"
 #include "mlos.h"
 
 #define LOADCAL_DEBUG    0
@@ -346,7 +345,7 @@ inv_error_t inv_store_calibration(void)
 
 free_mem_n_exit:
     inv_free(calData);
-    return INV_SUCCESS;
+    return result;
 }
 
 /**
diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
index a12a4ed..d0c4513 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -14,7 +14,13 @@ enum PROC_SYSFS_CMD {
 	CMD_GET_DEVICE_NODE
 };
 static char sysfs_path[100];
-static char *chip_name[] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050", "MPU6500"};
+static char *chip_name[] = {
+    "ITG3500", 
+    "MPU6050", 
+    "MPU9150", 
+    "MPU3050", 
+    "MPU6500"
+};
 static int chip_ind;
 static int initialized =0;
 static int status = 0;
@@ -26,7 +32,7 @@ static int iio_dev_num = 0;
 #define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
 #define FORMAT_TYPE_FILE "%s_type"
 
-#define CHIP_NUM sizeof(chip_name)/sizeof(char*)
+#define CHIP_NUM ARRAY_SIZE(chip_name)
 
 static const char *iio_dir = "/sys/bus/iio/devices/";
 
@@ -94,7 +100,7 @@ int find_type_by_name(const char *name, const char *type)
  */
 static int parsing_proc_input(int mode, char *name){
 	const char input[] = "/proc/bus/input/devices";
-	char line[100], d;
+	char line[4096], d;
 	char tmp[100];
 	FILE *fp;
 	int i, j, result, find_flag;
diff --git a/libsensors_iio/software/core/mllite/linux/mlos.h b/libsensors_iio/software/core/mllite/linux/mlos.h
index 287025f..d4f8912 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos.h
+++ b/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -10,6 +10,7 @@
 #ifndef __KERNEL__
 #include <stdio.h>
 #endif
+#include <pthread.h>
 
 #include "mltypes.h"
 
@@ -18,7 +19,7 @@ extern "C" {
 #endif
 
 #if defined(LINUX) || defined(__KERNEL__)
-typedef unsigned int HANDLE;
+typedef pthread_mutex_t* HANDLE;
 #endif
 
 	/* ------------ */
diff --git a/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
index 6c9a6ca..5424508 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos_linux.c
+++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -1,13 +1,14 @@
 /*
  $License:
-    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+    Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
  $
  */
+
 /*******************************************************************************
  *
  * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
  *
- *******************************************************************************/
+ ******************************************************************************/
 
 /**
  *  @defgroup MLOS
@@ -16,7 +17,7 @@
  *  @{
  *      @file mlos.c
  *      @brief OS Interface.
-**/
+ */
 
 /* ------------- */
 /* - Includes. - */
@@ -26,11 +27,10 @@
 #include <unistd.h>
 #include <pthread.h>
 #include <stdlib.h>
+#include <errno.h>
 
 #include "stdint_invensense.h"
-
 #include "mlos.h"
-#include <errno.h>
 
 
 /* -------------- */
@@ -39,14 +39,14 @@
 
 /**
  *  @brief  Allocate space
- *  @param  numBytes  number of bytes
+ *  @param  num_bytes  number of bytes
  *  @return pointer to allocated space
-**/
-void *inv_malloc(unsigned int numBytes)
+ */
+void *inv_malloc(unsigned int num_bytes)
 {
     // Allocate space.
-    void *allocPtr = malloc(numBytes);
-    return allocPtr;
+    void *alloc_ptr = malloc(num_bytes);
+    return alloc_ptr;
 }
 
 
@@ -54,14 +54,11 @@ void *inv_malloc(unsigned int numBytes)
  *  @brief  Free allocated space
  *  @param  ptr pointer to space to deallocate
  *  @return error code.
-**/
+ */
 inv_error_t inv_free(void *ptr)
 {
-    // Deallocate space.
-    if (ptr) {
-       free(ptr);
-    }
-
+    if (ptr)
+        free(ptr);
     return INV_SUCCESS;
 }
 
@@ -98,10 +95,10 @@ inv_error_t inv_create_mutex(HANDLE *mutex)
 inv_error_t inv_lock_mutex(HANDLE mutex)
 {
     int res;
-    pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
 
     res = pthread_mutex_lock(pm);
-    if(res == -1) 
+    if(res == -1)
         return INV_ERROR_OS_LOCK_FAILED;
 
     return INV_SUCCESS;
@@ -112,11 +109,11 @@ inv_error_t inv_lock_mutex(HANDLE mutex)
  *  @brief  Mutex unlock function
  *  @param  mutex   mutex handle
  *  @return error code.
-**/
+ */
 inv_error_t inv_unlock_mutex(HANDLE mutex)
 {
     int res;
-    pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
+    pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
 
     res = pthread_mutex_unlock(pm);
     if(res == -1) 
@@ -133,7 +130,7 @@ inv_error_t inv_unlock_mutex(HANDLE mutex)
  */
 FILE *inv_fopen(char *filename)
 {
-    FILE *fp = fopen(filename,"r");
+    FILE *fp = fopen(filename, "r");
     return fp;
 }
 
@@ -156,22 +153,21 @@ void inv_fclose(FILE *fp)
 inv_error_t inv_destroy_mutex(HANDLE handle)
 {
     int error;
-    pthread_mutex_t *pm = (pthread_mutex_t*)handle;
+    pthread_mutex_t *pm = (pthread_mutex_t *)handle;
     error = pthread_mutex_destroy(pm);
-    if (error) {
+    if (error)
         return errno;
-    }
     free((void*) handle);
-    
+
     return INV_SUCCESS;}
 
 
 /**
  *  @brief  Sleep function.
  */
-void inv_sleep(int mSecs)
+void inv_sleep(int m_secs)
 {
-    usleep(mSecs*1000);
+    usleep(m_secs * 1000);
 }
 
 
@@ -184,13 +180,11 @@ unsigned long inv_get_tick_count()
 {
     struct timeval tv;
 
-    if (gettimeofday(&tv, NULL) !=0)
+    if (gettimeofday(&tv, NULL) != 0)
         return 0;
 
     return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
 }
 
-  /**********************/
- /** @} */ /* defgroup */
-/**********************/
+/** @} */
 
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c
index d1fd9c4..c30d693 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.c
+++ b/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -670,7 +670,6 @@ void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
 
 void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
 {
-    float divider;
     pFilter->input = input;
     pFilter->output = input;
     pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c
index a8c253d..9141cc6 100644
--- a/libsensors_iio/software/core/mllite/mpl.c
+++ b/libsensors_iio/software/core/mllite/mpl.c
@@ -41,7 +41,7 @@ inv_error_t inv_init_mpl(void)
     return INV_SUCCESS;
 }
 
-const char ml_ver[] = "InvenSense MPL 5.1.1 RC6";
+const char ml_ver[] = "InvenSense MPL 5.1.2 beta RC9";
 
 /**
  *  @brief  used to get the MPL version.
diff --git a/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c
index 1484f9b..df58f40 100644
--- a/libsensors_iio/software/core/mllite/results_holder.c
+++ b/libsensors_iio/software/core/mllite/results_holder.c
@@ -13,6 +13,9 @@
  *       @file results_holder.c
  *       @brief Results Holder for HAL.
  */
+
+#include <string.h>
+
 #include "results_holder.h"
 #include "ml_math_func.h"
 #include "mlmath.h"
@@ -34,6 +37,7 @@ struct results_t {
     long mag_scale[3]; /**< scale factor to apply to magnetic field reading */
     long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */
     int acc_state; /**< Describes accel state */
+    int got_accel_bias; /**< Flag describing if accel bias is known */
     long compass_bias_error[3]; /**< Error Squared */
     unsigned char motion_state;
     unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */
@@ -318,6 +322,24 @@ inv_error_t inv_enable_results_holder()
     return result;
 }
 
+/** Sets state of if we know the accel bias.
+ * @return return 1 if we know the accel bias, 0 if not.
+ *            it is set with inv_set_accel_bias_found()
+ */
+int inv_got_accel_bias()
+{
+    return rh.got_accel_bias;
+}
+
+/** Sets whether we know the accel bias
+ * @param[in] state Set to 1 if we know the accel bias. 
+ *            Can be retrieved with inv_got_accel_bias()
+ */
+void inv_set_accel_bias_found(int state)
+{
+    rh.got_accel_bias = state;
+}
+
 /** Sets state of if we know the compass bias.
  * @return return 1 if we know the compass bias, 0 if not.
  *            it is set with inv_set_compass_bias_found()
diff --git a/libsensors_iio/software/core/mllite/results_holder.h b/libsensors_iio/software/core/mllite/results_holder.h
index a60d7f0..09da24e 100644
--- a/libsensors_iio/software/core/mllite/results_holder.h
+++ b/libsensors_iio/software/core/mllite/results_holder.h
@@ -70,6 +70,10 @@ inv_error_t inv_get_linear_accel_float(float *data);
 void inv_set_heading_confidence_interval(float ci);
 float inv_get_heading_confidence_interval(void);
 
+int inv_got_accel_bias();
+void inv_set_accel_bias_found(int state);
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c
index 721f858..3e4e619 100644
--- a/libsensors_iio/software/core/mllite/storage_manager.c
+++ b/libsensors_iio/software/core/mllite/storage_manager.c
@@ -14,6 +14,9 @@
  *       @file storage_manager.c
  *       @brief Load and Store Manager.
  */
+
+#include <string.h>
+
 #include "storage_manager.h"
 #include "log.h"
 #include "ml_math_func.h"
diff --git a/libsensors_iio/software/core/mpl/build/android/shared.mk b/libsensors_iio/software/core/mpl/build/android/shared.mk
index 79cf9c1..2e15205 100644
--- a/libsensors_iio/software/core/mpl/build/android/shared.mk
+++ b/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -16,6 +16,7 @@ MPL_DIR  = $(INV_ROOT)/software/core/mpl
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -43,11 +44,6 @@ LLINK += -ldl
 LFLAGS += $(CMDLINE_LFLAGS)
 LFLAGS += -shared 
 LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -nostdlib 
-LFLAGS += -fpic 
-LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc 
-LFLAGS += -Wl,--gc-sections 
-LFLAGS += -Wl,--no-whole-archive 
 LFLAGS += -Wl,-shared,-Bsymbolic 
 LFLAGS += $(ANDROID_LINK)
 LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
diff --git a/libsensors_iio/software/core/mpl/fast_no_motion.h b/libsensors_iio/software/core/mpl/fast_no_motion.h
index 2a33093..c553926 100644
--- a/libsensors_iio/software/core/mpl/fast_no_motion.h
+++ b/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -23,22 +23,24 @@ extern "C" {
     inv_error_t inv_enable_fast_nomot(void);
     inv_error_t inv_disable_fast_nomot(void);
     inv_error_t inv_start_fast_nomot(void);
-    inv_error_t inv_stop_fast_nomot(void);
-    inv_error_t inv_init_fast_nomot(void);
-    void inv_set_default_number_of_samples(int N);
-    inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);
-    inv_error_t inv_update_fast_nomot(long *gyro);
-    
-    void inv_get_fast_nomot_accel_param(long *cntr, float *param);
-    void inv_get_fast_nomot_compass_param(long *cntr, float *param);
-    void inv_set_fast_nomot_accel_threshold(float thresh);
-    void inv_set_fast_nomot_compass_threshold(float thresh);
-    void int_set_fast_nomot_gyro_threshold(float thresh);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_FAST_NO_MOTION_H__
-
+    inv_error_t inv_stop_fast_nomot(void);
+    inv_error_t inv_init_fast_nomot(void);
+    void inv_set_default_number_of_samples(int N);
+    inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);
+    inv_error_t inv_update_fast_nomot(long *gyro);
+
+    void inv_get_fast_nomot_accel_param(long *cntr, long long *param);
+    void inv_get_fast_nomot_compass_param(long *cntr, long long *param);
+    void inv_set_fast_nomot_accel_threshold(long long thresh);
+    void inv_set_fast_nomot_compass_threshold(long long thresh);
+    void int_set_fast_nomot_gyro_threshold(long long thresh);
+
+    void inv_fnm_debug_print(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FAST_NO_MOTION_H__
+
diff --git a/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
index 532e8af..4fa36b0 100644
--- a/libsensors_iio/software/core/mpl/quaternion_supervisor.h
+++ b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -18,6 +18,7 @@ inv_error_t inv_enable_quaternion(void);
 inv_error_t inv_disable_quaternion(void);
 inv_error_t inv_init_quaternion(void);
 inv_error_t inv_start_quaternion(void);
+void inv_set_quaternion(long *quat);
 
 #ifdef __cplusplus
 }
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared b/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
new file mode 100644
index 0000000..7635cd8
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
new file mode 100644
index 0000000..7655e4d
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
@@ -0,0 +1,95 @@
+EXEC = inv_gesture_test$(SHARED_APP_SUFFIX)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
+COMP  ?= $(CROSS)gcc
+LINK  ?= $(CROSS)gcc
+
+OBJFOLDER = $(CURDIR)/obj
+
+INV_ROOT   = ../../../../..
+APP_DIR    = $(CURDIR)/../..
+MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+MPL_DIR    = $(INV_ROOT)/software/core/mpl
+
+include $(INV_ROOT)/software/build/android/common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall
+CFLAGS += -fpic
+CFLAGS += -nostdlib
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT
+CFLAGS += -DLINUX
+CFLAGS += -DANDROID
+CFLAGS += -mthumb-interwork
+CFLAGS += -fno-exceptions
+CFLAGS += -ffunction-sections
+CFLAGS += -funwind-tables
+CFLAGS += -fstack-protector
+CFLAGS += -fno-short-enums
+CFLAGS += -fmessage-length=0
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MPL_DIR)
+CFLAGS += -I$(COMMON_DIR)
+CFLAGS += -I$(HAL_DIR)/include
+CFLAGS += $(INV_INCLUDES)
+CFLAGS += $(INV_DEFINES)
+
+LLINK  = -lc
+LLINK += -lm
+LLINK += -lutils
+LLINK += -lcutils
+LLINK += -lgcc
+LLINK += -ldl
+LLINK += -lstdc++
+LLINK += -llog
+LLINK += -lz
+
+LFLAGS += $(CMDLINE_LFLAGS)
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
+
+LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
+
+####################################################################################################
+## sources
+
+INV_LIBS  = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
+INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
+
+#INV_SOURCES and VPATH provided by Makefile.filelist
+include ../filelist.mk
+
+INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
+INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall install
+
+all: $(EXEC) $(MK_NAME)
+
+$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
+	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+
+$(OBJFOLDER) :
+	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+	mkdir obj
+
+$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c  $(MK_NAME)
+	@$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+	$(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $<
+
+clean : 
+	rm -fR $(OBJFOLDER)
+
+cleanall : 
+	rm -fR $(EXEC) $(OBJFOLDER)
+
+install : $(EXEC)
+	cp -f $(EXEC) $(INSTALL_DIR)
+
+
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
new file mode 100644
index 0000000..75d93cf
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
@@ -0,0 +1,11 @@
+#### filelist.mk for inv_gesture_test ####
+
+# headers
+#HEADERS += 
+
+# sources
+SOURCES := $(APP_DIR)/inv_gesture_test.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR)
diff --git a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
new file mode 100644
index 0000000..d38d478
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
@@ -0,0 +1,535 @@
+/**
+ *  Gesture Test application for Invensense's MPU6/9xxx (w/ DMP).
+ */
+
+#include <unistd.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <features.h>
+#include <dirent.h>
+#include <string.h>
+#include <poll.h>
+#include <stddef.h>
+#include <linux/input.h>
+#include <time.h>
+#include <linux/time.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include "invensense.h"
+#include "ml_math_func.h"
+#include "storage_manager.h"
+#include "ml_stored_data.h"
+#include "ml_sysfs_helper.h"
+
+#define DEBUG_PRINT     /* Uncomment to print Gyro & Accel read from Driver */
+
+#define MAX_SYSFS_NAME_LEN  (100)
+#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
+
+#define FLICK_UPPER_THRES       3147790
+#define FLICK_LOWER_THRES       -3147790
+#define FLICK_COUNTER           50
+#define POLL_TIME               2000 // 2sec
+
+#define FALSE   0
+#define TRUE    1
+
+char *sysfs_names_ptr;
+
+struct sysfs_attrbs {
+    char *enable;
+    char *power_state;
+    char *dmp_on;
+    char *dmp_int_on;
+    char *self_test;
+    char *dmp_firmware;
+    char *firmware_loaded;
+    char *display_orientation_on;
+    char *orientation_on;
+    char *event_flick;
+    char *event_display_orientation;
+    char *event_orientation;
+    char *event_tap;
+    char *flick_axis;
+    char *flick_counter;
+    char *flick_int_on;
+    char *flick_lower;
+    char *flick_upper;
+    char *flick_message_on;
+    char *tap_min_count;
+    char *tap_on;
+    char *tap_threshold;
+    char *tap_time;
+} mpu;
+
+enum {
+    tap,
+    flick,
+    gOrient,
+    orient,
+    numDMPFeatures
+};
+
+struct pollfd pfd[numDMPFeatures];
+
+/*******************************************************************************
+ *                       DMP Feature Supported Functions
+ ******************************************************************************/
+
+int read_sysfs_int(char *filename, int *var)
+{
+    int res=0;
+    FILE *fp;
+
+    fp = fopen(filename, "r");
+    if (fp!=NULL) {
+        fscanf(fp, "%d\n", var);
+	fclose(fp);
+    } else {
+        MPL_LOGE("ERR open file to read");
+        res= -1;
+    }
+    return res;
+}
+
+int write_sysfs_int(char *filename, int data)
+{
+    int res=0;
+    FILE  *fp;
+
+    fp = fopen(filename, "w");
+    if (fp!=NULL) {
+        fprintf(fp, "%d\n", data);
+	fclose(fp);
+    } else {
+        MPL_LOGE("ERR open file to write");
+        res= -1;
+    }
+    return res;
+}
+
+/**************************************************
+    This _kbhit() function is courtesy from Web
+***************************************************/
+int _kbhit() {
+    static const int STDIN = 0;
+    static bool initialized = false;
+
+    if (! initialized) {
+        // Use termios to turn off line buffering
+        struct termios term;
+        tcgetattr(STDIN, &term);
+        term.c_lflag &= ~ICANON;
+        tcsetattr(STDIN, TCSANOW, &term);
+        setbuf(stdin, NULL);
+        initialized = true;
+    }
+
+    int bytesWaiting;
+    ioctl(STDIN, FIONREAD, &bytesWaiting);
+    return bytesWaiting;
+}
+
+int inv_init_sysfs_attributes(void)
+{
+    unsigned char i = 0;
+    char sysfs_path[MAX_SYSFS_NAME_LEN];
+    char *sptr;
+    char **dptr;
+
+    sysfs_names_ptr =
+            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
+    sptr = sysfs_names_ptr;
+    if (sptr != NULL) {
+        dptr = (char**)&mpu;
+        do {
+            *dptr++ = sptr;
+            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
+        } while (++i < MAX_SYSFS_ATTRB);
+    } else {
+        MPL_LOGE("couldn't alloc mem for sysfs paths");
+        return -1;
+    }
+
+    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
+    inv_get_sysfs_path(sysfs_path);
+
+    sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable");
+    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
+    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
+    sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
+    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
+    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
+    sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
+    sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+    sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on");
+    sprintf(mpu.event_flick, "%s%s", sysfs_path, "/event_flick");
+    sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+    sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation");
+    sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap");
+    sprintf(mpu.flick_axis, "%s%s", sysfs_path, "/flick_axis");
+    sprintf(mpu.flick_counter, "%s%s", sysfs_path, "/flick_counter");
+    sprintf(mpu.flick_int_on, "%s%s", sysfs_path, "/flick_int_on");
+    sprintf(mpu.flick_lower, "%s%s", sysfs_path, "/flick_lower");
+    sprintf(mpu.flick_upper, "%s%s", sysfs_path, "/flick_upper");
+    sprintf(mpu.flick_message_on, "%s%s", sysfs_path, "/flick_message_on");
+    sprintf(mpu.tap_min_count, "%s%s", sysfs_path, "/tap_min_count");
+    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
+    sprintf(mpu.tap_threshold, "%s%s", sysfs_path, "/tap_threshold");
+    sprintf(mpu.tap_time, "%s%s", sysfs_path, "/tap_time");
+
+#if 0
+    // test print sysfs paths
+    dptr = (char**)&mpu;
+    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+        MPL_LOGE("sysfs path: %s", *dptr++);
+    }
+#endif
+    return 0;
+}
+
+int DmpFWloaded()
+{
+    int res;
+    read_sysfs_int(mpu.firmware_loaded, &res);
+    return res;
+}
+
+int enable_flick(int en)
+{
+   int res=0;
+   int flickUpper=0, flickLower=0, flickCounter=0;
+
+   if (write_sysfs_int(mpu.flick_int_on, en) < 0) {
+       printf("GT:ERR-can't write 'flick_int_on'");
+       res= -1;
+   }
+
+   if (en) {
+       flickUpper= FLICK_UPPER_THRES;
+       flickLower= FLICK_LOWER_THRES;
+       flickCounter= FLICK_COUNTER;
+   }
+
+   if (write_sysfs_int(mpu.flick_upper, flickUpper) < 0) {
+       printf("GT:ERR-can't write 'flick_upper'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_lower, flickLower) < 0) {
+       printf("GT:ERR-can't write 'flick_lower'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_counter, flickCounter) < 0) {
+       printf("GT:ERR-can't write 'flick_counter'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_message_on, 0) < 0) {
+       printf("GT:ERR-can't write 'flick_message_on'");
+       res= -1;
+   }
+
+   if (write_sysfs_int(mpu.flick_axis, 0) < 0) {
+       printf("GT:ERR_can't write 'flick_axis'");
+       res= -1;
+   }
+
+   return res;
+}
+
+int enable_tap(int en)
+{
+    if (write_sysfs_int(mpu.tap_on, en) < 0) {
+        printf("GT:ERR-can't write 'tap_on'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int enable_displ_orient(int en)
+{
+    if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'display_orientation_en'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int enable_orient(int en)
+{
+    if (write_sysfs_int(mpu.orientation_on, en) < 0) {
+        printf("GT:ERR-can't write 'orientation_on'");
+        return -1;
+    }
+
+    return 0;
+}
+
+int flickHandler()
+{
+    FILE *fp;
+    int data;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Flick Handler\n");
+#endif
+
+    fp = fopen(mpu.event_flick, "rt");
+    fscanf(fp, "%d\n", &data);
+    fclose (fp);
+
+    printf("Flick= %x\n", data);
+
+    return 0;
+}
+
+int tapHandler()
+{
+    FILE *fp;
+    int tap, tap_dir, tap_num;
+
+    fp = fopen(mpu.event_tap, "rt");
+    fscanf(fp, "%d\n", &tap);
+    fclose(fp);
+
+    tap_dir = tap/8;
+    tap_num = tap%8 + 1;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Tap Handler **\n");
+    printf("Tap= %x\n", tap);
+    printf("Tap Dir= %x\n", tap_dir);
+    printf("Tap Num= %x\n", tap_num);
+#endif
+
+    switch (tap_dir) {
+        case 1:
+            printf("Tap Axis->X Pos\n");
+            break;
+        case 2:
+            printf("Tap Axis->X Neg\n");
+            break;
+        case 3:
+            printf("Tap Axis->Y Pos\n");
+            break;
+        case 4:
+            printf("Tap Axis->Y Neg\n");
+            break;
+        case 5:
+            printf("Tap Axis->Z Pos\n");
+            break;
+        case 6:
+            printf("Tap Axis->Z Neg\n");
+            break;
+        default:
+            printf("Tap Axis->Unknown\n");
+            break;
+    }
+
+    return 0;
+}
+
+int googleOrientHandler()
+{
+    FILE *fp;
+    int orient;
+
+#ifdef DEBUG_PRINT
+    printf("GT:Google Orient Handler\n");
+#endif
+
+    fp = fopen(mpu.event_display_orientation, "rt");
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+    printf("Google Orient-> %d\n", orient);
+
+    return 0;
+}
+
+int orientHandler()
+{
+    FILE *fp;
+    int orient;
+
+    fp = fopen(mpu.event_orientation, "rt");
+    fscanf(fp, "%d\n", &orient);
+    fclose(fp);
+
+#ifdef DEBUG_PRINT
+    printf("GT:Reg Orient Handler\n");
+#endif
+
+    if (orient & 0x01)
+	printf("Orient->X Up\n");
+
+    if (orient & 0x02)
+	printf("Orient->X Down\n");
+
+    if (orient & 0x04)
+	printf("Orient->Y Up\n");
+
+    if (orient & 0x08)
+	printf("Orient->Y Down\n");
+
+    if (orient & 0x10)
+	printf("Orient->Z Up\n");
+
+    if (orient & 0x20)
+	printf("Orient->Z Down\n");
+
+    if (orient & 0x40)
+	printf("Orient->Flip\n");
+
+    return 0;
+}
+
+int enableDMPFeatures(int en)
+{
+    int res= -1;
+
+    if (DmpFWloaded())
+    {
+        /* Currently there's no info regarding DMP's supported features/capabilities */
+        /* An error in enabling features below could be an indication of the feature */
+        /* not supported in current loaded DMP firmware */
+
+        enable_flick(en);
+        enable_tap(en);
+        enable_displ_orient(en);
+        enable_orient(en);
+        res= 0;
+    }
+
+    return res;
+}
+
+int initFds()
+{
+    int i;
+
+    for (i=0; i< numDMPFeatures; i++) {
+        switch(i) {
+            case tap:
+                pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case flick:
+                pfd[i].fd = open(mpu.event_flick, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case gOrient:
+                pfd[i].fd = open(mpu.event_display_orientation, O_RDONLY | O_NONBLOCK);
+                break;
+
+            case orient:
+                pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK);
+                break;
+
+            default:
+                pfd[i].fd = -1;
+         }
+
+        pfd[i].events = POLLPRI|POLLERR,
+        pfd[i].revents = 0;
+#ifdef DEBUG_PRINT
+        printf("GT:pfd[%d].fd= %d\n", i, pfd[i].fd);
+#endif
+    }
+
+    return 0;
+}
+
+int closeFds()
+{
+    int i;
+    for (i = 0; i < numDMPFeatures; i++) {
+        if (!pfd[i].fd)
+            close(pfd[i].fd);
+    }
+    return 0;
+}
+
+/*******************************************************************************
+ *                       M a i n  S e l f  T e s t
+ ******************************************************************************/
+
+int main(int argc, char **argv)
+{
+    char data[4];
+    int i, res= 0;
+
+    res = inv_init_sysfs_attributes();
+    if (res) {
+        printf("GT:ERR-Can't allocate mem");
+        return -1;
+    }
+
+    /* On Gesture/DMP supported features */
+    enableDMPFeatures(1);
+
+    /* init Fds to poll for Gesture data */
+    initFds();
+
+    /* prompt user to make gesture and how to stop program */
+    printf("\n**Please make Gesture to see data.  Press any key to stop Prog**\n\n");
+
+    do {
+        for (i=0; i< numDMPFeatures; i++) {
+            read(pfd[i].fd, data, 4);
+        }
+
+        poll(pfd, numDMPFeatures, POLL_TIME);
+
+        for (i=0; i< numDMPFeatures; i++) {
+           if(pfd[i].revents != 0) {
+               switch(i) {
+                   case tap:
+                       tapHandler();
+                       break;
+
+                   case flick:
+                       flickHandler();
+                       break;
+
+                   case gOrient:
+                       googleOrientHandler();
+                       break;
+
+                   case orient:
+                       orientHandler();
+                       break;
+
+                   default:
+                       printf("GT:ERR-Not supported");
+                       break;
+               }
+               pfd[i].revents= 0;	//no need. reset anyway
+           }
+        }
+
+    } while (!_kbhit());
+
+    /* Off DMP features */
+    enableDMPFeatures(0);
+
+    /* release resources */
+    closeFds();
+    if (sysfs_names_ptr) {
+        free(sysfs_names_ptr);
+    }
+
+    printf("\nThank You!\n");
+
+    return res;
+}
+
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
index e20d640..b3d323c 100644
--- a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
+++ b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
@@ -33,6 +33,8 @@
 #include "ml_sysfs_helper.h"
 #include "authenticate.h"
 
+#define FLICK_SUPPORTED (0)
+
 /**
  * size_from_channelarray() - calculate the storage size of a scan
  * @channels: the channel info array
@@ -43,136 +45,139 @@
  **/
 int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
 {
-	int bytes = 0;
-	int i = 0;
-	while (i < num_channels) {
-		if (bytes % channels[i].bytes == 0)
-			channels[i].location = bytes;
-		else
-			channels[i].location = bytes - bytes%channels[i].bytes
-				+ channels[i].bytes;
-		bytes = channels[i].location + channels[i].bytes;
-		i++;
-	}
-	return bytes;
+    int bytes = 0;
+    int i = 0;
+    while (i < num_channels) {
+        if (bytes % channels[i].bytes == 0)
+            channels[i].location = bytes;
+        else
+            channels[i].location = bytes - bytes%channels[i].bytes
+                + channels[i].bytes;
+        bytes = channels[i].location + channels[i].bytes;
+        i++;
+    }
+    return bytes;
 }
 
 void print2byte(int input, struct iio_channel_info *info)
 {
-	/* shift before conversion to avoid sign extension
-	   of left aligned data */
-	input = input >> info->shift;
-	if (info->is_signed) {
-		int16_t val = input;
-		val &= (1 << info->bits_used) - 1;
-		val = (int16_t)(val << (16 - info->bits_used)) >>
-			(16 - info->bits_used);
-		/*printf("%d, %05f, scale=%05f", val,
-		       (float)(val + info->offset)*info->scale, info->scale);*/
-		printf("%d, ", val);
-
-	} else {
-		uint16_t val = input;
-		val &= (1 << info->bits_used) - 1;
-		printf("%05f ", ((float)val + info->offset)*info->scale);
-	}
+    /* shift before conversion to avoid sign extension
+       of left aligned data */
+    input = input >> info->shift;
+    if (info->is_signed) {
+        int16_t val = input;
+        val &= (1 << info->bits_used) - 1;
+        val = (int16_t)(val << (16 - info->bits_used)) >>
+            (16 - info->bits_used);
+        /*printf("%d, %05f, scale=%05f", val,
+               (float)(val + info->offset)*info->scale, info->scale);*/
+        printf("%d, ", val);
+
+    } else {
+        uint16_t val = input;
+        val &= (1 << info->bits_used) - 1;
+        printf("%05f ", ((float)val + info->offset)*info->scale);
+    }
 }
 /**
  * process_scan() - print out the values in SI units
- * @data:		pointer to the start of the scan
- * @infoarray:		information about the channels. Note
+ * @data:        pointer to the start of the scan
+ * @infoarray:        information about the channels. Note
  *  size_from_channelarray must have been called first to fill the
  *  location offsets.
- * @num_channels:	the number of active channels
+ * @num_channels:    the number of active channels
  **/
 void process_scan(char *data,
-		  struct iio_channel_info *infoarray,
-		  int num_channels)
+          struct iio_channel_info *infoarray,
+          int num_channels)
 {
-	int k;
-	//char *tmp;
-	for (k = 0; k < num_channels; k++) {
-		switch (infoarray[k].bytes) {
-			/* only a few cases implemented so far */
-		case 2:
-			print2byte(*(uint16_t *)(data + infoarray[k].location),
-				   &infoarray[k]);
-			//tmp = data + infoarray[k].location;
-			break;
-		case 4:
-			if (infoarray[k].is_signed) {
-				int32_t val = *(int32_t *)
-					(data +
-					 infoarray[k].location);
-				if ((val >> infoarray[k].bits_used) & 1)
-					val = (val & infoarray[k].mask) |
-						~infoarray[k].mask;
-				/* special case for timestamp */
-				printf(" %d ", val);
-			}
-			break;
-		case 8:
-			if (infoarray[k].is_signed) {
-				int64_t val = *(int64_t *)
-					(data +
-					 infoarray[k].location);
-				if ((val >> infoarray[k].bits_used) & 1)
-					val = (val & infoarray[k].mask) |
-						~infoarray[k].mask;
-				/* special case for timestamp */
-				if (infoarray[k].scale == 1.0f &&
-				    infoarray[k].offset == 0.0f)
-					printf(" %lld", val);
-				else
-					printf("%05f ", ((float)val +
-							 infoarray[k].offset)*
-					       infoarray[k].scale);
-			}
-			break;
-		default:
-			break;
-		}
-	}
-	printf("\n");
+    int k;
+    //char *tmp;
+    for (k = 0; k < num_channels; k++) {
+        switch (infoarray[k].bytes) {
+            /* only a few cases implemented so far */
+        case 2:
+            print2byte(*(uint16_t *)(data + infoarray[k].location),
+                   &infoarray[k]);
+            //tmp = data + infoarray[k].location;
+            break;
+        case 4:
+            if (infoarray[k].is_signed) {
+                int32_t val = *(int32_t *)
+                    (data +
+                     infoarray[k].location);
+                if ((val >> infoarray[k].bits_used) & 1)
+                    val = (val & infoarray[k].mask) |
+                        ~infoarray[k].mask;
+                /* special case for timestamp */
+                printf(" %d ", val);
+            }
+            break;
+        case 8:
+            if (infoarray[k].is_signed) {
+                int64_t val = *(int64_t *)
+                    (data +
+                     infoarray[k].location);
+                if ((val >> infoarray[k].bits_used) & 1)
+                    val = (val & infoarray[k].mask) |
+                        ~infoarray[k].mask;
+                /* special case for timestamp */
+                if (infoarray[k].scale == 1.0f &&
+                    infoarray[k].offset == 0.0f)
+                    printf(" %lld", val);
+                else
+                    printf("%05f ", ((float)val +
+                             infoarray[k].offset)*
+                           infoarray[k].scale);
+            }
+            break;
+        default:
+            break;
+        }
+    }
+    printf("\n");
 }
 
-void enable_flick(char *p){
-	int ret;
-	printf("flick:%s\n", p);
-	ret = write_sysfs_int_and_verify("flick_int_on", p, 1);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("flick_upper", p, 3147790);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("flick_lower", p, -3147790);
-	if (ret < 0)
-		return;
-
-	ret = write_sysfs_int_and_verify("flick_counter", p, 50);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("flick_message_on", p, 0);
-	if (ret < 0)
-		return;
-	ret = write_sysfs_int_and_verify("flick_axis", p, 0);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+void enable_flick(char *p, int on){
+    int ret;
+    printf("flick:%s\n", p);
+    ret = write_sysfs_int_and_verify("flick_int_on", p, on);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_upper", p, 3147790);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_lower", p, -3147790);
+    if (ret < 0)
+        return;
+
+    ret = write_sysfs_int_and_verify("flick_counter", p, 50);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_message_on", p, 0);
+    if (ret < 0)
+        return;
+    ret = write_sysfs_int_and_verify("flick_axis", p, 0);
 }
+#endif
+
 void HandleOrient(int orient)
 {    
     if (orient & 0x01)
-	printf("INV_X_UP\n");
+    printf("INV_X_UP\n");
     if (orient & 0x02) 
-	printf("INV_X_DOWN\n");
+    printf("INV_X_DOWN\n");
     if (orient & 0x04) 
-	printf("INV_Y_UP\n");
+    printf("INV_Y_UP\n");
     if (orient & 0x08) 
-	printf("INV_Y_DOWN\n");
+    printf("INV_Y_DOWN\n");
     if (orient & 0x10) 
-	printf("INV_Z_UP\n");
+    printf("INV_Z_UP\n");
     if (orient & 0x20) 
-	printf("INV_Z_DOWN\n");
+    printf("INV_Z_DOWN\n");
     if (orient & 0x40) 
-	printf("INV_ORIENTATION_FLIP\n");
+    printf("INV_ORIENTATION_FLIP\n");
 }
 
 void HandleTap(int tap)
@@ -204,21 +209,44 @@ void HandleTap(int tap)
     }
     printf("Tap number: %d\n", tap_num);
 }
-void setup_dmp(char *dev_path){
-	char sysfs_path[200];
-	char dmp_path[200];
-	int  ret;
-	FILE *fd;
-	sprintf(sysfs_path, "%s", dev_path);
-	printf("sysfs: %s\n", sysfs_path);
-	ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1);
-	if (ret < 0)
-		return;
+#define DMP_CODE_SIZE 3060
+void verify_img(char *dmp_path){
+    FILE *fp;
+    int i;
+    char dmp_img[DMP_CODE_SIZE];
+    if ((fp = fopen(dmp_path, "rb")) < 0 ) {
+        perror("dmp fail");
+    }
+    i = fread(dmp_img, 1, DMP_CODE_SIZE, fp);
+    printf("Result=%d\n", i);
+    fclose(fp);
+    fp = fopen("/dev/read_img.h", "wt");
+    fprintf(fp, "char rec[]={\n");
+    for(i=0; i<DMP_CODE_SIZE; i++) {
+      fprintf(fp, "0x%02x, ", dmp_img[i]);
+      if(((i+1)%16) == 0) {
+        fprintf(fp, "\n");
+      }
+    }
+    fprintf(fp, "};\n ");
+    fclose(fp);
+}
+
+void setup_dmp(char *dev_path, int p_event){
+    char sysfs_path[200];
+    char dmp_path[200];
+    int  ret;
+    FILE *fd;
+    sprintf(sysfs_path, "%s", dev_path);
+    printf("sysfs: %s\n", sysfs_path);
+    ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1);
+    if (ret < 0)
+        return;
 
 	ret = write_sysfs_int("in_accel_scale", dev_path, 0);
 	if (ret < 0)
 		return;
-	ret = write_sysfs_int("in_anglvel_scale", dev_path, 3);
+	ret = write_sysfs_int("in_anglvel_scale", dev_path, 2);
 	if (ret < 0)
 		return;	
 	ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
@@ -234,349 +262,424 @@ void setup_dmp(char *dev_path){
 	inv_load_dmp(fd);
 	fclose(fd);
 	printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
+	ret = write_sysfs_int_and_verify("in_accel_x_offset", sysfs_path, 0xabcd0000);
+	ret = write_sysfs_int_and_verify("in_accel_y_offset", sysfs_path, 0xffff0000);
+	ret = write_sysfs_int_and_verify("in_accel_z_offset", sysfs_path, 0xcdef0000);
+
 	ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1);
 	if (ret < 0)
 		return;
 	ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1);
 	if (ret < 0)
 		return;
-	ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1);
+	/* selelct which event to enable and interrupt on/off here */
+	//enable_flick(sysfs_path, 1);
+	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1);
 	if (ret < 0)
 		return;
-	enable_flick(sysfs_path);
-	ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1);
+	ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1);
 	if (ret < 0)
 		return;
 	ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1);
 	if (ret < 0)
 		return;
+	printf("rate\n");
+	ret = write_sysfs_int_and_verify("dmp_output_rate", sysfs_path, 25);
+	if (ret < 0)
+		return;
+	ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, p_event);
+	if (ret < 0)
+		return;
+	//verify_img(dmp_path);
 }
 
-void get_dmp_event(char *dev_dir_name) {
-	char file_name[100];
-	int i;
-	int fp_flick, fp_tap, fp_orient, fp_disp;
-	int data;
-	char d[4];
-	FILE *fp;
-	struct pollfd pfd[4];
-	printf("%s\n", dev_dir_name);
-	while(1) {
-		sprintf(file_name, "%s/event_flick", dev_dir_name);
-		fp_flick = open(file_name, O_RDONLY | O_NONBLOCK);
-		sprintf(file_name, "%s/event_tap", dev_dir_name);
-		fp_tap = open(file_name, O_RDONLY | O_NONBLOCK);
-		sprintf(file_name, "%s/event_orientation", dev_dir_name);
-		fp_orient = open(file_name, O_RDONLY | O_NONBLOCK);
-		sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
-		fp_disp = open(file_name, O_RDONLY | O_NONBLOCK);
-
-		pfd[0].fd = fp_flick;
-		pfd[0].events = POLLPRI|POLLERR,
-		pfd[0].revents = 0;			
-
-		pfd[1].fd = fp_tap;
-		pfd[1].events = POLLPRI|POLLERR,
-		pfd[1].revents = 0;			
-
-		pfd[2].fd = fp_orient;
-		pfd[2].events = POLLPRI|POLLERR,
-		pfd[2].revents = 0;			
-
-		pfd[3].fd = fp_disp;
-		pfd[3].events = POLLPRI|POLLERR,
-		pfd[3].revents = 0;			
-
-		read(fp_flick, d, 4);
-		read(fp_tap, d, 4);
-		read(fp_orient, d, 4);
-		read(fp_disp, d, 4);
-
-		poll(pfd, 4, -1);
-		close(fp_flick);
-		close(fp_tap);
-		close(fp_orient);
-		close(fp_disp);
-
-		for (i=0; i< ARRAY_SIZE(pfd); i++) {
-			if(pfd[i].revents != 0) {
-				switch (i){
-				case 0:
-					sprintf(file_name, "%s/event_flick", dev_dir_name);
-					fp = fopen(file_name, "rt");
-					fscanf(fp, "%d\n", &data);
-					printf("flick=%x\n", data);
-					fclose(fp);
-				break;
-				case 1:
-					sprintf(file_name, "%s/event_tap", dev_dir_name);
-					fp = fopen(file_name, "rt");
-					fscanf(fp, "%d\n", &data);
-					printf("tap=%x\n", data);
-					HandleTap(data);
-					fclose(fp);
-				break;
-				case 2:
-					sprintf(file_name, "%s/event_orientation", dev_dir_name);
-					fp = fopen(file_name, "rt");
-					fscanf(fp, "%d\n", &data);
-					printf("orient=%x\n", data);
-					HandleOrient(data);
-					fclose(fp);
-				break;
-				case 3:
-					sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
-					fp = fopen(file_name, "rt");
-					fscanf(fp, "%d\n", &data);
-					printf("display_orient=%x\n", data);
-					fclose(fp);
-				break;
-				}
-			}
-		}						
-	}
+void get_dmp_event(char *dev_dir_name)
+{
+    char file_name[100];
+    int i;
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+    int fp_tap, fp_orient, fp_disp, fp_flick;
+    const int n_gest = 6;
+#else
+    int fp_tap, fp_orient, fp_disp, fp_motion;
+    //int fp_no_motion;
+    const int n_gest = 4;
+#endif
+    int data;
+    char d[6];
+    FILE *fp;
+    struct pollfd pfd[4];
+    printf("%s\n", dev_dir_name);
+    while(1) {
+        sprintf(file_name, "%s/event_tap", dev_dir_name);
+        fp_tap = open(file_name, O_RDONLY | O_NONBLOCK);
+        sprintf(file_name, "%s/event_orientation", dev_dir_name);
+        fp_orient = open(file_name, O_RDONLY | O_NONBLOCK);
+        sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
+        fp_disp = open(file_name, O_RDONLY | O_NONBLOCK);
+
+        //sprintf(file_name, "%s/event_accel_motion", dev_dir_name);
+        sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
+        fp_motion = open(file_name, O_RDONLY | O_NONBLOCK);
+        //sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
+        //fp_no_motion = open(file_name, O_RDONLY | O_NONBLOCK);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        sprintf(file_name, "%s/event_flick", dev_dir_name);
+        fp_flick = open(file_name, O_RDONLY | O_NONBLOCK);
+#endif
+
+        pfd[0].fd = fp_tap;
+        pfd[0].events = POLLPRI|POLLERR,
+        pfd[0].revents = 0;
+
+        pfd[1].fd = fp_orient;
+        pfd[1].events = POLLPRI|POLLERR,
+        pfd[1].revents = 0;
+
+        pfd[2].fd = fp_disp;
+        pfd[2].events = POLLPRI|POLLERR,
+        pfd[2].revents = 0;
+
+        pfd[3].fd = fp_motion;
+        pfd[3].events = POLLPRI|POLLERR,
+        pfd[3].revents = 0;
+
+        //pfd[4].fd = fp_no_motion;
+        //pfd[4].events = POLLPRI|POLLERR,
+        //pfd[4].revents = 0;
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        pfd[5].fd = fp_flick;
+        pfd[5].events = POLLPRI|POLLERR,
+        pfd[5].revents = 0;    
+#endif
+
+        read(fp_tap, d, 4);
+        read(fp_orient, d, 4);
+        read(fp_disp, d, 4);
+        read(fp_motion, d, 4);
+        //read(fp_no_motion, d, 4);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        read(fp_flick, d, 4);
+#endif
+
+        poll(pfd, n_gest, -1);
+        close(fp_tap);
+        close(fp_orient);
+        close(fp_disp);
+        close(fp_motion);
+        //close(fp_no_motion);
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+        close(fp_flick);
+#endif
+        for (i = 0; i < ARRAY_SIZE(pfd); i++) {
+            if(pfd[i].revents != 0) {
+                switch (i){
+                case 0:
+                    sprintf(file_name, "%s/event_tap", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("tap=%x\n", data);
+                    HandleTap(data);
+                    fclose(fp);
+                break;
+                case 1:
+                    sprintf(file_name, "%s/event_orientation", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("orient=%x\n", data);
+                    HandleOrient(data);
+                    fclose(fp);
+                break;
+                case 2:
+                    sprintf(file_name, "%s/event_display_orientation", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("display_orient=%x\n", data);
+                    fclose(fp);
+                break;
+                case 3:
+                    sprintf(file_name, "%s/event_accel_wom", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("motion=%x\n", data);
+                    fclose(fp);
+                break;
+                case 4:
+                    sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("No motion=%x\n", data);
+                    fclose(fp);
+                break;
+
+#if FLICK_SUPPORTED /* hide flick, not offially supported */
+                case 5:
+                    sprintf(file_name, "%s/event_flick", dev_dir_name);
+                    fp = fopen(file_name, "rt");
+                    fscanf(fp, "%d\n", &data);
+                    printf("flick=%x\n", data);
+                    fclose(fp);
+                break;
+#endif
+                }
+            }
+        }                        
+    }
 }
 
 
 int main(int argc, char **argv)
 {
-	unsigned long num_loops = 2;
-	unsigned long timedelay = 100000;
-	unsigned long buf_len = 128;
-
-	int ret, c, i, j, toread;
-	int fp;
-
-	int num_channels;
-	char *trigger_name = NULL;
-	char *dev_dir_name, *buf_dir_name;
-
-	int datardytrigger = 1;
-	char *data;
-	int read_size;
-	int dev_num, trig_num;
-	char *buffer_access;
-	int scan_size;
-	int noevents = 0;
-	int p_event = 0, nodmp = 0;
-	char *dummy;
-	char chip_name[10];
-	char device_name[10];
-	char sysfs[100];
-
-	struct iio_channel_info *infoarray;
-	/*set r means no DMP is enabled. should be used for mpu3050.
-	  set p means no print of data*/
-	/* when p is set, 1 means orientation, 2 means tap, 3 means flick*/
-	while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) {
-		switch (c) {
-		case 't':
-			trigger_name = optarg;
-			datardytrigger = 0;
-			break;
-		case 'e':
-			noevents = 1;
-			break;
-		case 'p':
-			p_event = 1;
-			break;
-		case 'r':
-			nodmp = 1;
-			break;
-		case 'c':
-			num_loops = strtoul(optarg, &dummy, 10);
-			break;
-		case 'w':
-			timedelay = strtoul(optarg, &dummy, 10);
-			break;
-		case 'l':
-			buf_len = strtoul(optarg, &dummy, 10);
-			break;
-		case '?':
-			return -1;
-		}
-	}
-	inv_get_sysfs_path(sysfs);
-	printf("sss:::%s\n", sysfs);
-	if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
-		printf("get chip name fail\n");
-		exit(0);
-	}
-	printf("chip_name=%s\n", chip_name);
-	if (INV_SUCCESS != inv_check_key())
-        	printf("key check fail\n");
-	else
-        	printf("key authenticated\n");
-
-	for (i=0; i<strlen(chip_name); i++) {
-		device_name[i] = tolower(chip_name[i]);
-	}
-	device_name[strlen(chip_name)] = '\0';
-	printf("device name: %s\n", device_name);
-
-	/* Find the device requested */
-	dev_num = find_type_by_name(device_name, "iio:device");
-	if (dev_num < 0) {
-		printf("Failed to find the %s\n", device_name);
-		ret = -ENODEV;
-		goto error_ret;
-	}
-	printf("iio device number being used is %d\n", dev_num);
-
-	asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
-	if (trigger_name == NULL) {
-		/*
-		 * Build the trigger name. If it is device associated it's
-		 * name is <device_name>_dev[n] where n matches the device
-		 * number found above
-		 */
-		ret = asprintf(&trigger_name,
-			       "%s-dev%d", device_name, dev_num);
-		if (ret < 0) {
-			ret = -ENOMEM;
-			goto error_ret;
-		}
-	}
-	ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1);
-	ret = write_sysfs_int_and_verify("firmware_loaded", dev_dir_name, 0);
-
-	/* Verify the trigger exists */
-	trig_num = find_type_by_name(trigger_name, "trigger");
-	if (trig_num < 0) {
-		printf("Failed to find the trigger %s\n", trigger_name);
-		ret = -ENODEV;
-		goto error_free_triggername;
-	}
-	printf("iio trigger number being used is %d\n", trig_num);
-	/*
-	 * Parse the files in scan_elements to identify what channels are
-	 * present
-	 */
-	ret = 0;
-	ret = enable(dev_dir_name, &infoarray, &num_channels);
-	if (ret) {
-		printf("error enable\n");
-		goto error_free_triggername;
-	}
-
-	if (nodmp ==0)
-		setup_dmp(dev_dir_name);
-
-	/*
-	 * Construct the directory name for the associated buffer.
-	 * As we know that the lis3l02dq has only one buffer this may
-	 * be built rather than found.
-	 */
-	ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_triggername;
-	}
-	printf("%s %s\n", dev_dir_name, trigger_name);
-
-	/* Set the device trigger to be the data rdy trigger found above */
-	ret = write_sysfs_string_and_verify("trigger/current_trigger",
-					dev_dir_name,
-					trigger_name);
-	if (ret < 0) {
-		printf("Failed to write current_trigger file\n");
-		goto error_free_buf_dir_name;
-	}
-	/* Setup ring buffer parameters */
-	/* 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);
-	if (ret < 0)
-		goto exit_here;
+    unsigned long num_loops = 2;
+    unsigned long timedelay = 100000;
+    unsigned long buf_len = 128;
+
+    int ret, c, i, j, toread;
+    int fp;
+
+    int num_channels;
+    char *trigger_name = NULL;
+    char *dev_dir_name, *buf_dir_name;
+
+    int datardytrigger = 1;
+    char *data;
+    int read_size;
+    int dev_num, trig_num;
+    char *buffer_access;
+    int scan_size;
+    int noevents = 0;
+    int p_event = 0, nodmp = 0;
+    char *dummy;
+    char chip_name[10];
+    char device_name[10];
+    char sysfs[100];
+
+    struct iio_channel_info *infoarray;
+    /* -r means no DMP is enabled (raw) -> should be used for mpu3050.
+       -p means no print of data */
+    /* when using -p, 1 means orientation, 2 means tap, 3 means flick */
+    while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) {
+        switch (c) {
+        case 't':
+            trigger_name = optarg;
+            datardytrigger = 0;
+            break;
+        case 'e':
+            noevents = 1;
+            break;
+        case 'p':
+            p_event = 1;
+            break;
+        case 'r':
+            nodmp = 1;
+            break;
+        case 'c':
+            num_loops = strtoul(optarg, &dummy, 10);
+            break;
+        case 'w':
+            timedelay = strtoul(optarg, &dummy, 10);
+            break;
+        case 'l':
+            buf_len = strtoul(optarg, &dummy, 10);
+            break;
+        case '?':
+            return -1;
+        }
+    }
+    inv_get_sysfs_path(sysfs);
+    printf("sss:::%s\n", sysfs);
+    if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
+        printf("get chip name fail\n");
+        exit(0);
+    }
+    printf("chip_name=%s\n", chip_name);
+    if (INV_SUCCESS != inv_check_key())
+            printf("key check fail\n");
+    else
+            printf("key authenticated\n");
+
+    for (i=0; i<strlen(chip_name); i++) {
+        device_name[i] = tolower(chip_name[i]);
+    }
+    device_name[strlen(chip_name)] = '\0';
+    printf("device name: %s\n", device_name);
+
+    /* Find the device requested */
+    dev_num = find_type_by_name(device_name, "iio:device");
+    if (dev_num < 0) {
+        printf("Failed to find the %s\n", device_name);
+        ret = -ENODEV;
+        goto error_ret;
+    }
+    printf("iio device number being used is %d\n", dev_num);
+    asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
+    if (trigger_name == NULL) {
+        /*
+         * Build the trigger name. If it is device associated it's
+         * name is <device_name>_dev[n] where n matches the device
+         * number found above
+         */
+        ret = asprintf(&trigger_name,
+                   "%s-dev%d", device_name, dev_num);
+        if (ret < 0) {
+            ret = -ENOMEM;
+            goto error_ret;
+        }
+    }
+    ret = write_sysfs_int("buffer/enable", dev_dir_name, 0);
+
+	ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1);
 	ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
 	ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
 	ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1);
-	if (nodmp == 0)
-		ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1);
-	else
-		ret = disable_q_out(dev_dir_name, &infoarray, &num_channels);		
-	ret = build_channel_array(dev_dir_name, &infoarray, &num_channels);
-	if (ret) {
-		printf("Problem reading scan element information\n");
-		goto exit_here;
-	}
-
-	/* Enable the buffer */
-	ret = write_sysfs_int("enable", buf_dir_name, 1);
-	if (ret < 0)
-		goto exit_here;
-	scan_size = size_from_channelarray(infoarray, num_channels);
-	data = malloc(scan_size*buf_len);
-	if (!data) {
-		ret = -ENOMEM;
-		goto exit_here;
-	}
-
-	ret = asprintf(&buffer_access,
-		       "/dev/iio:device%d",
-		       dev_num);
-	if (ret < 0) {
-		ret = -ENOMEM;
-		goto error_free_data;
-	}
-	if (p_event) {
-		get_dmp_event(dev_dir_name);
-		goto error_free_buffer_access;
-	}
-	/* Attempt to open non blocking the access dev */
-	fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
-	if (fp == -1) { /*If it isn't there make the node */
-		printf("Failed to open %s\n", buffer_access);
-		ret = -errno;
-		goto error_free_buffer_access;
-	}
-	/* Wait for events 10 times */
-	for (j = 0; j < num_loops; j++) {
-		if (!noevents) {
-			struct pollfd pfd = {
-				.fd = fp,
-				.events = POLLIN,
-			};
-			poll(&pfd, 1, -1);
-			toread = 1;
-			if ((j%128)==0)
-				usleep(timedelay);
-
-		} else {
-			usleep(timedelay);
-			toread = 1;
-		}
-		read_size = read(fp,
-				 data,
-				 toread*scan_size);
-		if (read_size == -EAGAIN) {
-			printf("nothing available\n");
-			continue;
-		}
-		if (0 == p_event) {
-			for (i = 0; i < read_size/scan_size; i++)
-				process_scan(data + scan_size*i,
-					     infoarray,
-					     num_channels);
-		}
-	}
-	close(fp);
+/*
+	ret = write_sysfs_int_and_verify("zero_motion_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("zero_motion_dur", dev_dir_name, 12);
+	ret = write_sysfs_int_and_verify("zero_motion_threshold", dev_dir_name, 13);
+
+	ret = write_sysfs_int_and_verify("motion_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("motion_dur", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("motion_threshold", dev_dir_name, 1);
+*/
+	ret = write_sysfs_int_and_verify("accel_wom_on", dev_dir_name, 1);
+	ret = write_sysfs_int_and_verify("accel_wom_threshold", dev_dir_name, 100);
+    /* Verify the trigger exists */
+    trig_num = find_type_by_name(trigger_name, "trigger");
+    if (trig_num < 0) {
+        printf("Failed to find the trigger %s\n", trigger_name);
+        ret = -ENODEV;
+        goto error_free_triggername;
+    }
+    printf("iio trigger number being used is %d\n", trig_num);
+    /*
+     * Parse the files in scan_elements to identify what channels are
+     * present
+     */
+    ret = 0;
+    ret = enable(dev_dir_name, &infoarray, &num_channels);
+    if (ret) {
+        printf("error enable\n");
+        goto error_free_triggername;
+    }
+    if (!nodmp)
+        setup_dmp(dev_dir_name, p_event);
+
+    /*
+     * Construct the directory name for the associated buffer.
+     * As we know that the lis3l02dq has only one buffer this may
+     * be built rather than found.
+     */
+    ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_free_triggername;
+    }
+    printf("%s %s\n", dev_dir_name, trigger_name);
+
+    /* Set the device trigger to be the data rdy trigger found above */
+    ret = write_sysfs_string_and_verify("trigger/current_trigger",
+                    dev_dir_name,
+                    trigger_name);
+    if (ret < 0) {
+        printf("Failed to write current_trigger file\n");
+        goto error_free_buf_dir_name;
+    }
+    /* Setup ring buffer parameters */
+    /* 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);
+    if (ret < 0)
+        goto exit_here;
+    ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1);
+    ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1);
+    //ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 0);
+    if (nodmp == 0) {
+        ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1);
+    } else {
+        ret = disable_q_out(dev_dir_name, &infoarray, &num_channels);        
+        ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0);
+    }
+    ret = build_channel_array(dev_dir_name, &infoarray, &num_channels);
+    if (ret) {
+        printf("Problem reading scan element information\n");
+        goto exit_here;
+    }
+
+    /* Enable the buffer */
+    ret = write_sysfs_int("enable", buf_dir_name, 1);
+    if (ret < 0)
+        goto exit_here;
+    scan_size = size_from_channelarray(infoarray, num_channels);
+    data = malloc(scan_size*buf_len);
+    if (!data) {
+        ret = -ENOMEM;
+        goto exit_here;
+    }
+
+    ret = asprintf(&buffer_access,
+               "/dev/iio:device%d",
+               dev_num);
+    if (ret < 0) {
+        ret = -ENOMEM;
+        goto error_free_data;
+    }
+    if (p_event) {
+        get_dmp_event(dev_dir_name);
+        goto error_free_buffer_access;
+    }
+    /* Attempt to open non blocking the access dev */
+    fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
+    if (fp == -1) { /*If it isn't there make the node */
+        printf("Failed to open %s\n", buffer_access);
+        ret = -errno;
+        goto error_free_buffer_access;
+    }
+    /* Wait for events 10 times */
+    for (j = 0; j < num_loops; j++) {
+        if (!noevents) {
+            struct pollfd pfd = {
+                .fd = fp,
+                .events = POLLIN,
+            };
+            poll(&pfd, 1, -1);
+            toread = 1;
+            if ((j%128)==0)
+                usleep(timedelay);
+
+        } else {
+            usleep(timedelay);
+            toread = 1;
+        }
+        read_size = read(fp,
+                 data,
+                 toread*scan_size);
+        if (read_size == -EAGAIN) {
+            printf("nothing available\n");
+            continue;
+        }
+        if (0 == p_event) {
+            for (i = 0; i < read_size/scan_size; i++)
+                process_scan(data + scan_size*i,
+                         infoarray,
+                         num_channels);
+        }
+    }
+    close(fp);
 error_free_buffer_access:
-	free(buffer_access);
+    free(buffer_access);
 error_free_data:
-	free(data);
+    free(data);
 exit_here:
-	/* Stop the ring buffer */
-	ret = write_sysfs_int("enable", buf_dir_name, 0);
+    /* Stop the ring buffer */
+    ret = write_sysfs_int("enable", buf_dir_name, 0);
 
 error_free_buf_dir_name:
-	free(buf_dir_name);
+    free(buf_dir_name);
 error_free_triggername:
-	if (datardytrigger)
-		free(trigger_name);
+    if (datardytrigger)
+        free(trigger_name);
 error_ret:
-	return ret;
+    return ret;
 }
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
index 3a055cc..ed5fbf6 100644
--- a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk
@@ -18,6 +18,7 @@ HAL_DIR    = $(INV_ROOT)/software/core/HAL
 include $(INV_ROOT)/software/build/android/common.mk
 
 CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += $(ANDROID_COMPILE)
 CFLAGS += -Wall
 CFLAGS += -fpic
 CFLAGS += -nostdlib
@@ -49,20 +50,8 @@ LLINK += -lstdc++
 LLINK += -llog
 LLINK += -lz
 
-PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
-PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
-PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
-
 LFLAGS += $(CMDLINE_LFLAGS)
-LFLAGS += -nostdlib
-LFLAGS += -fpic
-LFLAGS += -Wl,--gc-sections 
-LFLAGS += -Wl,--no-whole-archive 
-LFLAGS += -Wl,-dynamic-linker,/system/bin/linker
-LFLAGS += $(ANDROID_LINK)
-ifneq ($(PRODUCT),panda)
-LFLAGS += -rdynamic
-endif
+LFLAGS += $(ANDROID_LINK_EXECUTABLE)
 
 LRPATH  = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
 
@@ -87,7 +76,7 @@ all: $(EXEC) $(MK_NAME)
 
 $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME)
 	@$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n")
-	$(LINK) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
+	$(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH)
 
 $(OBJFOLDER) :
 	@$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
diff --git a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
index f3eadc4..87ed703 100644
--- a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
+++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c
@@ -115,7 +115,7 @@ int read_sysfs_int(char *filename, int *var)
         fscanf(fp, "%d\n", var);
         fclose(fp);
     } else {
-        LOGE("HAL:ERR open file to read");
+        MPL_LOGE("inv_self_test: ERR open file to read");
         res= -1;   
     }
     return res;
@@ -132,7 +132,7 @@ int write_sysfs_int(char *filename, int data)
         fprintf(fp, "%d\n", data);
         fclose(fp);
     } else {
-        LOGE("HAL:ERR open file to write");
+        MPL_LOGE("inv_self_test: ERR open file to write");
         res= -1;   
     }
     return res;
@@ -155,7 +155,7 @@ int inv_init_sysfs_attributes(void)
             sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
         } while (++i < MAX_SYSFS_ATTRB);
     } else {
-        LOGE("HAL:couldn't alloc mem for sysfs paths");
+        MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths");
         return -1;
     }
 
@@ -184,7 +184,7 @@ int inv_init_sysfs_attributes(void)
     // test print sysfs paths
     dptr = (char**)&mpu;
     for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
-        LOGE("HAL:sysfs path: %s", *dptr++);
+        MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++);
     }
 #endif
     return 0;
@@ -407,6 +407,7 @@ int main(int argc, char **argv)
             result = -1;
         }
     }
+
     free(buffer);
 
 free_sysfs_storage:
author	Ricardo Cerqueira <cyanogenmod@cerqueira.org>	2013-07-24 23:01:58 +0100
committer	Ricardo Cerqueira <cyanogenmod@cerqueira.org>	2013-07-24 23:01:58 +0100
commit	de880a60e91e342912110aecc5f15385d791aa2a (patch)
tree	d76c01ea1e0b220d8136d5f819da146946a958c7
parent	33ce91b37062fa63af192f5643de93f3beebe854 (diff)
parent	60c042a376b34a7f4343752b96c1bc0bf4da31cc (diff)
download	android_hardware_invensense-cm-10.2.0.tar.gz android_hardware_invensense-cm-10.2.0.tar.bz2 android_hardware_invensense-cm-10.2.0.zip