Revert "Official MA 5.1.2 - RC11 Release"
This reverts commit 6a4d9a48ffde124c498496f6dae1e77c2cae6864
Change-Id: I85c4b326a2adf930fcd4a64e4c4f36e99f4fa4cb
diff --git a/Android.mk b/Android.mk
index c42f281..427bbb4 100644
--- a/Android.mk
+++ b/Android.mk
@@ -1,5 +1,5 @@
# Can't have both manta and non-manta libsensors.
-ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
+ifneq ($(filter manta, $(TARGET_DEVICE)),)
# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta.
include $(call all-named-subdir-makefiles,libsensors_iio)
else
diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk
index d05ec7b..c3b2003 100644
--- a/libsensors_iio/Android.mk
+++ b/libsensors_iio/Android.mk
@@ -15,28 +15,26 @@
LOCAL_PATH := $(call my-dir)
+ifneq ($(TARGET_SIMULATOR),true)
+
# InvenSense fragment of the HAL
include $(CLEAR_VARS)
LOCAL_MODULE := libinvensense_hal
LOCAL_MODULE_TAGS := optional
-LOCAL_MODULE_OWNER := invensense
+
LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
-VERSION_JB := true
-ifeq ($(VERSION_JB),true)
-LOCAL_CFLAGS += -DANDROID_JELLYBEAN
-endif
-ifeq ($(TARGET_BUILD_VARIANT),userdebug)
+ifeq ($(ENG_BUILD),1)
ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
endif
ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
endif
-ifeq ($(COMPILE_COMPASS_YAS53x),1)
-LOCAL_CFLAGS += -DCOMPASS_YAS53x
+ifeq ($(COMPILE_COMPASS_YAS530),1)
+LOCAL_CFLAGS += -DCOMPASS_YAS530
endif
ifeq ($(COMPILE_COMPASS_AK8975),1)
LOCAL_CFLAGS += -DCOMPASS_AK8975
@@ -48,31 +46,13 @@
LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
endif
-
-LOCAL_SRC_FILES += SensorBase.cpp
+LOCAL_SRC_FILES := SensorBase.cpp
LOCAL_SRC_FILES += MPLSensor.cpp
LOCAL_SRC_FILES += MPLSupport.cpp
LOCAL_SRC_FILES += InputEventReader.cpp
-
-
-ifeq ($(TARGET_BUILD_VARIANT),userdebug)
-ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
-ifeq ($(COMPILE_COMPASS_AMI306),1)
-LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
-else ifeq ($(COMPILE_COMPASS_YAS53x),1)
-LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp
-else
LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
-endif
-else # COMPILE_INVENSENSE_COMPASS_CAL = 0
-# choose corresponding 3rd-party compass sensor file
-LOCAL_SRC_FILES += AkmSensor.cpp
-LOCAL_SRC_FILES += CompassSensor.AKM.cpp
-endif
-else # release builds, default
-LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
-endif #userdebug
+LOCAL_C_INCLUDES += $(LOCAL_PATH)
LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
@@ -83,6 +63,7 @@
LOCAL_SHARED_LIBRARIES += libutils
LOCAL_SHARED_LIBRARIES += libdl
LOCAL_SHARED_LIBRARIES += libmllite
+
#Additions for SysPed
LOCAL_SHARED_LIBRARIES += libmplmpu
LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
@@ -91,70 +72,10 @@
include $(BUILD_SHARED_LIBRARY)
-# Build a temporary HAL that links the InvenSense .so
-include $(CLEAR_VARS)
-ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
-LOCAL_MODULE := sensors.invensense
-else
-LOCAL_MODULE := sensors.${TARGET_PRODUCT}
-endif
-LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw
-
-LOCAL_SHARED_LIBRARIES += libmplmpu
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
-
-LOCAL_PRELINK_MODULE := false
-LOCAL_MODULE_TAGS := optional
-LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\"
-
-ifeq ($(VERSION_JB),true)
-LOCAL_CFLAGS += -DANDROID_JELLYBEAN
-endif
-
-ifeq ($(TARGET_BUILD_VARIANT),userdebug)
-ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
-LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
-endif
-ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
-LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
-endif
-ifeq ($(COMPILE_COMPASS_YAS53x),1)
-LOCAL_CFLAGS += -DCOMPASS_YAS53x
-endif
-ifeq ($(COMPILE_COMPASS_AK8975),1)
-LOCAL_CFLAGS += -DCOMPASS_AK8975
-endif
-ifeq ($(COMPILE_COMPASS_AMI306),1)
-LOCAL_CFLAGS += -DCOMPASS_AMI306
-endif
-else # release builds, default
-LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
-endif # userdebug
-
-ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),)
-#LOCAL_SRC_FILES := sensors_mpl.cpp
-else
-LOCAL_SRC_FILES := sensors_mpl.cpp
-endif
-
-
-LOCAL_SHARED_LIBRARIES := libinvensense_hal
-LOCAL_SHARED_LIBRARIES += libcutils
-LOCAL_SHARED_LIBRARIES += libutils
-LOCAL_SHARED_LIBRARIES += libdl
-LOCAL_SHARED_LIBRARIES += liblog
-LOCAL_SHARED_LIBRARIES += libmllite
-include $(BUILD_SHARED_LIBRARY)
-
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
@@ -165,9 +86,10 @@
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
OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
include $(BUILD_PREBUILT)
+
+endif # !TARGET_SIMULATOR
diff --git a/libsensors_iio/CompassSensor.IIO.9150.cpp b/libsensors_iio/CompassSensor.IIO.9150.cpp
index b11d5cb..d9f2e0c 100644
--- a/libsensors_iio/CompassSensor.IIO.9150.cpp
+++ b/libsensors_iio/CompassSensor.IIO.9150.cpp
@@ -31,11 +31,12 @@
#include "MPLSupport.h"
#include "sensor_params.h"
#include "ml_sysfs_helper.h"
+#include "local_log_def.h"
#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
-#if defined COMPASS_YAS53x
-# warning "Invensense compass cal with YAS53x IIO on secondary bus"
+#if defined COMPASS_YAS530
+# warning "Invensense compass cal with YAS530 on primary bus"
# define USE_MPL_COMPASS_HAL (1)
# define COMPASS_NAME "INV_YAS530"
#elif defined COMPASS_AK8975
@@ -43,7 +44,7 @@
# define USE_MPL_COMPASS_HAL (1)
# define COMPASS_NAME "INV_AK8975"
#elif defined INVENSENSE_COMPASS_CAL
-# warning "Invensense compass cal with compass IIO on secondary bus"
+# warning "Invensense compass cal with compass on secondary bus"
# define USE_MPL_COMPASS_HAL (1)
# define COMPASS_NAME "INV_COMPASS"
#else
@@ -56,11 +57,11 @@
/*****************************************************************************/
-CompassSensor::CompassSensor()
+CompassSensor::CompassSensor()
: SensorBase(NULL, NULL),
- compass_fd(-1),
mCompassTimestamp(0),
- mCompassInputReader(8)
+ mCompassInputReader(8),
+ compass_fd(-1)
{
VFUNC_LOG;
@@ -77,13 +78,13 @@
memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
compassSysFs.compass_orient, getTimestamp());
FILE *fptr;
fptr = fopen(compassSysFs.compass_orient, "r");
if (fptr != NULL) {
int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+ fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
&om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
&om[6], &om[7], &om[8]);
fclose(fptr);
@@ -106,9 +107,7 @@
LOGE("HAL:Couldn't read compass mounting matrix");
}
- if (!isIntegrated()) {
- enable(ID_M, 0);
- }
+ enable(ID_M, 0);
}
CompassSensor::~CompassSensor()
@@ -131,11 +130,11 @@
* @param[in] handle
* which sensor to enable/disable.
* @param[in] en
- * en=1, enable;
+ * en=1, enable;
* en=0, disable
* @return if the operation is successful.
*/
-int CompassSensor::enable(int32_t handle, int en)
+int CompassSensor::enable(int32_t handle, int en)
{
VFUNC_LOG;
@@ -143,7 +142,7 @@
int tempFd;
int res;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_enable, getTimestamp());
tempFd = open(compassSysFs.compass_enable, O_RDWR);
res = errno;
@@ -157,7 +156,7 @@
close(tempFd);
if (en) {
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_x_fifo_enable, getTimestamp());
tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
res = errno;
@@ -169,7 +168,7 @@
compassSysFs.compass_x_fifo_enable, strerror(res), res);
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_y_fifo_enable, getTimestamp());
tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
res = errno;
@@ -181,7 +180,7 @@
compassSysFs.compass_y_fifo_enable, strerror(res), res);
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, compassSysFs.compass_z_fifo_enable, getTimestamp());
tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
res = errno;
@@ -197,13 +196,13 @@
return res;
}
-int CompassSensor::setDelay(int32_t handle, int64_t ns)
+int CompassSensor::setDelay(int32_t handle, int64_t ns)
{
VFUNC_LOG;
int tempFd;
int res;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
mDelay = ns;
if (ns == 0)
@@ -247,8 +246,8 @@
mCachedCompassData[2] = event->value;
break;
}
-
- mCompassTimestamp =
+
+ mCompassTimestamp =
(int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
}
@@ -263,7 +262,7 @@
VFUNC_LOG;
long sensitivity;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
compassSysFs.compass_scale, getTimestamp());
inv_read_data(compassSysFs.compass_scale, &sensitivity);
return sensitivity;
@@ -311,7 +310,7 @@
/**
* @brief This function will return the current delay for this sensor.
- * @return delay in nanoseconds.
+ * @return delay in nanoseconds.
*/
int64_t CompassSensor::getDelay(int32_t handle)
{
@@ -334,20 +333,14 @@
return;
}
if(!strcmp(compass, "compass")
- || !strcmp(compass, "INV_AK8975") ) {
+ || !strcmp(compass, "INV_AK8975")
+ || !strcmp(compass, "INV_YAS530")) {
list->maxRange = COMPASS_AKM8975_RANGE;
list->resolution = COMPASS_AKM8975_RESOLUTION;
list->power = COMPASS_AKM8975_POWER;
list->minDelay = COMPASS_AKM8975_MINDELAY;
return;
}
- if(!strcmp(compass, "INV_YAS530")) {
- 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")) {
list->maxRange = COMPASS_AMI306_RANGE;
list->resolution = COMPASS_AMI306_RESOLUTION;
@@ -390,16 +383,23 @@
// get proper (in absolute/relative) IIO path & build MPU's sysfs paths
// inv_get_sysfs_abs_path(sysfs_path);
- inv_get_sysfs_path(sysfs_path);
+ if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
+ ALOGE("CompassSensor failed get sysfs path");
+ return -1;
+ }
+
inv_get_iio_trigger_path(iio_trigger_path);
-#if defined COMPASS_AK8975
+#if defined COMPASS_YAS530 || defined COMPASS_AK8975
inv_get_input_number(COMPASS_NAME, &num);
tbuf[0] = num + 0x30;
tbuf[1] = 0;
sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
+#if defined COMPASS_YAS530
+ strcat(sysfs_path, "/yas530");
+#else
strcat(sysfs_path, "/ak8975");
-
+#endif
sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
@@ -415,7 +415,7 @@
#endif
#if 0
- // test print sysfs paths
+ // 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/CompassSensor.IIO.primary.cpp b/libsensors_iio/CompassSensor.IIO.primary.cpp
deleted file mode 100644
index 8b78338..0000000
--- a/libsensors_iio/CompassSensor.IIO.primary.cpp
+++ /dev/null
@@ -1,630 +0,0 @@
-/*
- * Copyright (C) 2012 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#define LOG_NDEBUG 0
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <unistd.h>
-#include <dirent.h>
-#include <sys/select.h>
-#include <cutils/log.h>
-#include <linux/input.h>
-#include <string.h>
-
-#include "CompassSensor.IIO.primary.h"
-#include "sensors.h"
-#include "MPLSupport.h"
-#include "sensor_params.h"
-#include "ml_sysfs_helper.h"
-//#include "log.h"
-
-#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
-
-#if defined COMPASS_AK8975
-# warning "Invensense compass cal with AK8975 on primary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "INV_AK8975"
-#elif defined INVENSENSE_COMPASS_CAL
-
-#if defined COMPASS_YAS53x
-# warning "Invensense compass cal with YAS53x IIO on primary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "yas53" // prefix for YAS53[023]
-#elif defined COMPASS_AMI306
-# warning "Invensense compass cal with AMI306 IIO on primary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "ami306"
-#else
-# warning "Invensense compass cal with compass on secondary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "INV_COMPASS"
-#endif
-
-#else
-# warning "third party compass cal HAL"
-# define USE_MPL_COMPASS_HAL (0)
-// TODO: change to vendor's name
-# define COMPASS_NAME "AKM8975"
-#endif
-
-/******************************************************************************/
-
-CompassSensor::CompassSensor()
- : SensorBase(COMPASS_NAME, NULL),
- mCompassTimestamp(0),
- mCompassInputReader(8)
-#ifdef COMPASS_YAS53x
- , mCoilsResetFd(0)
-#endif
-{
- FILE *fptr;
-
- VFUNC_LOG;
-
- /*
- char temp_sysfs_path[30];
- inv_get_sysfs_path(temp_sysfs_path);
- LOGI("sysfs: %s", temp_sysfs_path);
- */
-
-#ifdef COMPASS_YAS53x
- /* for YAS53x compasses, dev_name is just a prefix,
- we need to find the actual name */
- if (fill_dev_full_name_by_prefix(dev_name,
- dev_full_name, sizeof(dev_full_name) / sizeof(dev_full_name[0]))) {
- LOGE("Cannot find Yamaha device with prefix name '%s' - "
- "magnetometer will likely not work.", dev_name);
- }
-#else
- strncpy(dev_full_name, dev_name,
- sizeof(dev_full_name) / sizeof(dev_full_name[0]));
-#endif
-
- if (inv_init_sysfs_attributes()) {
- LOGE("Error Instantiating Compass\n");
- return;
- }
-
- if (!strcmp(dev_full_name, "INV_COMPASS")) {
- mI2CBus = COMPASS_BUS_SECONDARY;
- } else {
- mI2CBus = COMPASS_BUS_PRIMARY;
- }
-
- memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
-
- if (!isIntegrated()) {
- enable(ID_M, 0);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:compass name: %s", dev_full_name);
- enable_iio_sysfs();
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- compassSysFs.compass_orient, getTimestamp());
- fptr = fopen(compassSysFs.compass_orient, "r");
- if (fptr != NULL) {
- int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
- &om[6], &om[7], &om[8]);
- fclose(fptr);
-
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:compass mounting matrix: "
- "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
- om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
-
- mCompassOrientation[0] = om[0];
- mCompassOrientation[1] = om[1];
- mCompassOrientation[2] = om[2];
- mCompassOrientation[3] = om[3];
- mCompassOrientation[4] = om[4];
- mCompassOrientation[5] = om[5];
- mCompassOrientation[6] = om[6];
- mCompassOrientation[7] = om[7];
- mCompassOrientation[8] = om[8];
- } else {
- LOGE("HAL:Couldn't read compass mounting matrix");
- }
-
-#ifdef COMPASS_YAS53x
- mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+");
- if (fptr == NULL) {
- LOGE("HAL:Couldn't read compass overunderflow");
- }
-#endif
-}
-
-void CompassSensor::enable_iio_sysfs()
-{
- VFUNC_LOG;
-
- int tempFd = 0;
- char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
- FILE *tempFp = NULL;
- const char* compass = dev_full_name;
-
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:echo 1 > %s (%lld)",
- compassSysFs.in_timestamp_en, getTimestamp());
- tempFd = open(compassSysFs.in_timestamp_en, O_RDWR);
- if(tempFd < 0) {
- LOGE("HAL:could not open %s timestamp enable", compass);
- } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
- LOGE("HAL:could not enable timestamp");
- }
-
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)",
- compassSysFs.trigger_name, getTimestamp());
- tempFp = fopen(compassSysFs.trigger_name, "r");
- if (tempFp == NULL) {
- LOGE("HAL:could not open %s trigger name", compass);
- } else {
- if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
- LOGE("HAL:could not read trigger name");
- }
- fclose(tempFp);
- }
-
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:echo %s > %s (%lld)",
- iio_trigger_name, compassSysFs.current_trigger, getTimestamp());
- tempFp = fopen(compassSysFs.current_trigger, "w");
- 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");
- }
- fclose(tempFp);
- }
-
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:echo %d > %s (%lld)",
- IIO_BUFFER_LENGTH, compassSysFs.buffer_length, getTimestamp());
- tempFp = fopen(compassSysFs.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);
- }
-
- sprintf(iio_device_node, "%s%d", "/dev/iio:device",
- find_type_by_name(compass, "iio:device"));
- compass_fd = open(iio_device_node, O_RDONLY);
- int res = errno;
- if (compass_fd < 0) {
- LOGE("HAL:could not open '%s' iio device node in path '%s' - "
- "error '%s' (%d)",
- compass, iio_device_node, strerror(res), res);
- } else {
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:iio %s, compass_fd opened : %d", compass, compass_fd);
- }
-
- /* TODO: need further tests for optimization to reduce context-switch
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
- compassSysFs.compass_x_fifo_enable, getTimestamp());
- tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, 1);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.compass_x_fifo_enable, strerror(res), res);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
- compassSysFs.compass_y_fifo_enable, getTimestamp());
- tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, 1);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.compass_y_fifo_enable, strerror(res), res);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
- compassSysFs.compass_z_fifo_enable, getTimestamp());
- tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, 1);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.compass_z_fifo_enable, strerror(res), res);
- }
- */
-}
-
-CompassSensor::~CompassSensor()
-{
- VFUNC_LOG;
-
- free(pathP);
- if( compass_fd > 0)
- close(compass_fd);
-#ifdef COMPASS_YAS53x
- if( mCoilsResetFd != NULL )
- fclose(mCoilsResetFd);
-#endif
-}
-
-int CompassSensor::getFd() const
-{
- VHANDLER_LOG;
- return compass_fd;
-}
-
-/**
- * @brief This function will enable/disable sensor.
- * @param[in] handle
- * which sensor to enable/disable.
- * @param[in] en
- * en=1, enable;
- * en=0, disable
- * @return if the operation is successful.
- */
-int CompassSensor::enable(int32_t handle, int en)
-{
- VFUNC_LOG;
-
- mEnable = en;
- int tempFd;
- int res;
-
- /* reset master enable */
- res = masterEnable(0);
- if (res < 0) {
- return res;
- }
-
- if (en) {
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, compassSysFs.compass_x_fifo_enable, getTimestamp());
- tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.compass_x_fifo_enable, strerror(res), res);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, compassSysFs.compass_y_fifo_enable, getTimestamp());
- tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.compass_y_fifo_enable, strerror(res), res);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, compassSysFs.compass_z_fifo_enable, getTimestamp());
- tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.compass_z_fifo_enable, strerror(res), res);
- }
-
- res = masterEnable(en);
- if (res < en) {
- return res;
- }
- }
-
- return res;
-}
-
-int CompassSensor::masterEnable(int en) {
- VFUNC_LOG;
-
- int res = 0;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, compassSysFs.chip_enable, getTimestamp());
- int tempFd = open(compassSysFs.chip_enable, O_RDWR);
- res = errno;
- if(tempFd < 0){
- LOGE("HAL:open of %s failed with '%s' (%d)",
- compassSysFs.chip_enable, strerror(res), res);
- return res;
- }
- res = enable_sysfs_sensor(tempFd, en);
- return res;
-}
-
-int CompassSensor::setDelay(int32_t handle, int64_t ns)
-{
- VFUNC_LOG;
- int tempFd;
- int res;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
- mDelay = ns;
- if (ns == 0)
- return -1;
- tempFd = open(compassSysFs.compass_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, 1000000000.f / ns);
- if(res < 0) {
- LOGE("HAL:Compass update delay error");
- }
- return res;
-}
-
-/**
- @brief This function will return the state of the sensor.
- @return 1=enabled; 0=disabled
-**/
-int CompassSensor::getEnable(int32_t handle)
-{
- VFUNC_LOG;
- return mEnable;
-}
-
-/* use for Invensense compass calibration */
-#define COMPASS_EVENT_DEBUG (0)
-void CompassSensor::processCompassEvent(const input_event *event)
-{
- VHANDLER_LOG;
-
- switch (event->code) {
- case EVENT_TYPE_ICOMPASS_X:
- LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
- mCachedCompassData[0] = event->value;
- break;
- case EVENT_TYPE_ICOMPASS_Y:
- LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
- mCachedCompassData[1] = event->value;
- break;
- case EVENT_TYPE_ICOMPASS_Z:
- LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
- mCachedCompassData[2] = event->value;
- break;
- }
-
- mCompassTimestamp =
- (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
-}
-
-void CompassSensor::getOrientationMatrix(signed char *orient)
-{
- VFUNC_LOG;
- memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
-}
-
-long CompassSensor::getSensitivity()
-{
- VFUNC_LOG;
-
- long sensitivity;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- compassSysFs.compass_scale, getTimestamp());
- inv_read_data(compassSysFs.compass_scale, &sensitivity);
- return sensitivity;
-}
-
-/**
- @brief This function is called by sensors_mpl.cpp
- to read sensor data from the driver.
- @param[out] data sensor data is stored in this variable. Scaled such that
- 1 uT = 2^16
- @para[in] timestamp data's timestamp
- @return 1, if 1 sample read, 0, if not, negative if error
- */
-int CompassSensor::readSample(long *data, int64_t *timestamp) {
- VFUNC_LOG;
-
- int i;
- char *rdata = mIIOBuffer;
-
- size_t rsize = read(compass_fd, rdata, (8 * mEnable + 8) * 1);
-
- if (!mEnable) {
- rsize = read(compass_fd, rdata, (8 + 8) * IIO_BUFFER_LENGTH);
- // LOGI("clear buffer with size: %d", rsize);
- }
-/*
- LOGI("get one sample of AMI IIO data with size: %d", rsize);
- LOGI_IF(mEnable, "compass x/y/z: %d/%d/%d", *((short *) (rdata + 0)),
- *((short *) (rdata + 2)), *((short *) (rdata + 4)));
-*/
- if (mEnable) {
- for (i = 0; i < 3; i++) {
- data[i] = *((short *) (rdata + i * 2));
- }
- *timestamp = *((long long *) (rdata + 8 * mEnable));
- }
-
- return mEnable;
-}
-
-/**
- * @brief This function will return the current delay for this sensor.
- * @return delay in nanoseconds.
- */
-int64_t CompassSensor::getDelay(int32_t handle)
-{
- VFUNC_LOG;
- return mDelay;
-}
-
-void CompassSensor::fillList(struct sensor_t *list)
-{
- VFUNC_LOG;
-
- const char *compass = dev_full_name;
-
- if (compass) {
- if(!strcmp(compass, "INV_COMPASS")) {
- list->maxRange = COMPASS_MPU9150_RANGE;
- list->resolution = COMPASS_MPU9150_RESOLUTION;
- list->power = COMPASS_MPU9150_POWER;
- list->minDelay = COMPASS_MPU9150_MINDELAY;
- return;
- }
- if(!strcmp(compass, "compass")
- || !strcmp(compass, "INV_AK8975")) {
- list->maxRange = COMPASS_AKM8975_RANGE;
- list->resolution = COMPASS_AKM8975_RESOLUTION;
- list->power = COMPASS_AKM8975_POWER;
- list->minDelay = COMPASS_AKM8975_MINDELAY;
- return;
- }
- if(!strcmp(compass, "ami306")) {
- list->maxRange = COMPASS_AMI306_RANGE;
- list->resolution = COMPASS_AMI306_RESOLUTION;
- list->power = COMPASS_AMI306_POWER;
- list->minDelay = COMPASS_AMI306_MINDELAY;
- return;
- }
- if(!strcmp(compass, "yas530")
- || !strcmp(compass, "yas532")
- || !strcmp(compass, "yas533")) {
- list->maxRange = COMPASS_YAS53x_RANGE;
- list->resolution = COMPASS_YAS53x_RESOLUTION;
- list->power = COMPASS_YAS53x_POWER;
- list->minDelay = COMPASS_YAS53x_MINDELAY;
- return;
- }
- }
-
- LOGE("HAL:unknown compass id %s -- "
- "params default to ak8975 and might be wrong.",
- compass);
- list->maxRange = COMPASS_AKM8975_RANGE;
- list->resolution = COMPASS_AKM8975_RESOLUTION;
- list->power = COMPASS_AKM8975_POWER;
- list->minDelay = COMPASS_AKM8975_MINDELAY;
-}
-
-#ifdef COMPASS_YAS53x
-/* Read sysfs entry to determine whether overflow had happend
- then write to sysfs to reset to zero */
-int CompassSensor::checkCoilsReset()
-{
- int result=-1;
- VFUNC_LOG;
-
- if(mCoilsResetFd != NULL) {
- int attr;
- rewind(mCoilsResetFd);
- fscanf(mCoilsResetFd, "%d", &attr);
- if(attr == 0)
- return 0;
- else {
- LOGV_IF(SYSFS_VERBOSE, "HAL:overflow detected");
- rewind(mCoilsResetFd);
- if( fprintf(mCoilsResetFd, "%d", 0) < 0) {
- LOGE("HAL:could not write overunderflow");
- }
- else
- return 1;
- }
- }
- else {
- LOGE("HAL:could not read overunderflow");
- }
- return result;
-}
-#endif
-
-int CompassSensor::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];
- char *sptr;
- char **dptr;
- int num;
- const char* compass = dev_full_name;
-
- pathP = (char*)malloc(
- sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
- sptr = pathP;
- dptr = (char**)&compassSysFs;
- if (sptr == NULL)
- return -1;
-
- do {
- *dptr++ = sptr;
- sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
- } while (++i < COMPASS_MAX_SYSFS_ATTRB);
-
- // get proper (in absolute/relative) IIO path & build sysfs paths
- sprintf(sysfs_path, "%s%d", "/sys/bus/iio/devices/iio:device",
- find_type_by_name(compass, "iio:device"));
- sprintf(iio_trigger_path, "%s%d", "/sys/bus/iio/devices/trigger",
- find_type_by_name(compass, "iio:device"));
-
-#if defined COMPASS_AK8975
- inv_get_input_number(compass, &num);
- tbuf[0] = num + 0x30;
- tbuf[1] = 0;
- sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
- strcat(sysfs_path, "/ak8975");
-
- sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
- sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
- sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
- sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
-#else /* IIO */
- sprintf(compassSysFs.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
- sprintf(compassSysFs.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
- sprintf(compassSysFs.trigger_name, "%s%s", iio_trigger_path, "/name");
- sprintf(compassSysFs.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
- sprintf(compassSysFs.buffer_length, "%s%s", sysfs_path, "/buffer/length");
-
- sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en");
- sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en");
- sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en");
- sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency");
- sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
- sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
-
-#if defined COMPASS_YAS53x
- sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow");
-#endif
-#endif
-
-#if 0
- // test print sysfs paths
- dptr = (char**)&compassSysFs;
- LOGI("sysfs path base: %s", sysfs_path);
- LOGI("trigger sysfs path base: %s", iio_trigger_path);
- for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
- LOGE("HAL:sysfs path: %s", *dptr++);
- }
-#endif
- return 0;
-}
diff --git a/libsensors_iio/CompassSensor.IIO.primary.h b/libsensors_iio/CompassSensor.IIO.primary.h
deleted file mode 100644
index 844615c..0000000
--- a/libsensors_iio/CompassSensor.IIO.primary.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * Copyright (C) 2012 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef COMPASS_SENSOR_H
-#define COMPASS_SENSOR_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-// TODO fixme, need input_event
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <poll.h>
-#include <utils/Vector.h>
-#include <utils/KeyedVector.h>
-
-#include "sensors.h"
-#include "SensorBase.h"
-#include "InputEventReader.h"
-
-#define MAX_CHIP_ID_LEN (20)
-
-class CompassSensor : public SensorBase {
-
-public:
- CompassSensor();
- virtual ~CompassSensor();
-
- virtual int getFd() const;
- virtual int enable(int32_t handle, int enabled);
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int getEnable(int32_t handle);
- virtual int64_t getDelay(int32_t handle);
-
- // unnecessary for MPL
- virtual int readEvents(sensors_event_t *data, int count) { return 0; }
-
- int readSample(long *data, int64_t *timestamp);
- int providesCalibration() { return 0; }
- void getOrientationMatrix(signed char *orient);
- long getSensitivity();
- int getAccuracy() { return 0; }
- void fillList(struct sensor_t *list);
- int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); }
-#ifdef COMPASS_YAS53x
- int checkCoilsReset(void);
-#endif
-
-private:
- enum CompassBus {
- COMPASS_BUS_PRIMARY = 0,
- COMPASS_BUS_SECONDARY = 1
- } mI2CBus;
-
- struct sysfs_attrbs {
- char *chip_enable;
- char *in_timestamp_en;
- char *trigger_name;
- char *current_trigger;
- char *buffer_length;
-
- char *compass_enable;
- char *compass_x_fifo_enable;
- char *compass_y_fifo_enable;
- char *compass_z_fifo_enable;
- char *compass_rate;
- char *compass_scale;
- char *compass_orient;
-#ifdef COMPASS_YAS53x
- char *compass_attr_1;
-#endif
- } compassSysFs;
-
- char dev_full_name[20];
-
- // implementation specific
- signed char mCompassOrientation[9];
- long mCachedCompassData[3];
- int64_t mCompassTimestamp;
- InputEventCircularReader mCompassInputReader;
- int compass_fd;
- int64_t mDelay;
- int mEnable;
- char *pathP;
-
- char mIIOBuffer[(8 + 8) * IIO_BUFFER_LENGTH];
-
- int masterEnable(int en);
- void enable_iio_sysfs(void);
- void processCompassEvent(const input_event *event);
- int inv_init_sysfs_attributes(void);
-
-#ifdef COMPASS_YAS53x
- FILE *mCoilsResetFd;
-#endif
-};
-
-/*****************************************************************************/
-
-#endif // COMPASS_SENSOR_H
diff --git a/libsensors_iio/InputEventReader.cpp b/libsensors_iio/InputEventReader.cpp
index d3e0fe6..ca0a61a 100644
--- a/libsensors_iio/InputEventReader.cpp
+++ b/libsensors_iio/InputEventReader.cpp
@@ -1,104 +1,105 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define LOG_NDEBUG 0
-
-#include <stdint.h>
-#include <errno.h>
-#include <unistd.h>
-#include <poll.h>
-
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <cutils/log.h>
-
-#include "InputEventReader.h"
-
-/*****************************************************************************/
-
-struct input_event;
-
-InputEventCircularReader::InputEventCircularReader(size_t numEvents)
- : mBuffer(new input_event[numEvents * 2]),
- mBufferEnd(mBuffer + numEvents),
- mHead(mBuffer),
- mCurr(mBuffer),
- mFreeSpace(numEvents)
-{
-}
-
-InputEventCircularReader::~InputEventCircularReader()
-{
- delete [] mBuffer;
-}
-
-#define INPUT_EVENT_DEBUG (0)
-ssize_t InputEventCircularReader::fill(int fd)
-{
- size_t numEventsRead = 0;
- LOGV_IF(INPUT_EVENT_DEBUG,
- "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
- if (mFreeSpace) {
- const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
- if (nread < 0 || nread % sizeof(input_event)) {
- //LOGE("Partial event received nread=%d, required=%d",
- // nread, sizeof(input_event));
- //LOGE("FD trying to read is: %d");
- // we got a partial event!!
- if (INPUT_EVENT_DEBUG) {
- LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n",
- __PRETTY_FUNCTION__);
- LOGV_IF(nread % sizeof(input_event),
- "DEBUG:%s exit nread %% sizeof(input_event)\n",
- __PRETTY_FUNCTION__);
- }
- return (nread < 0 ? -errno : -EINVAL);
- }
-
- numEventsRead = nread / sizeof(input_event);
- if (numEventsRead) {
- mHead += numEventsRead;
- mFreeSpace -= numEventsRead;
- if (mHead > mBufferEnd) {
- size_t s = mHead - mBufferEnd;
- memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
- mHead = mBuffer + s;
- }
- }
- }
-
- LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);
- return numEventsRead;
-}
-
-ssize_t InputEventCircularReader::readEvent(input_event const** events)
-{
- *events = mCurr;
- ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
- return available ? 1 : 0;
-}
-
-void InputEventCircularReader::next()
-{
- mCurr++;
- mFreeSpace++;
- if (mCurr >= mBufferEnd) {
- mCurr = mBuffer;
- }
-}
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+
+#include <stdint.h>
+#include <errno.h>
+#include <unistd.h>
+#include <poll.h>
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <cutils/log.h>
+
+#include "InputEventReader.h"
+#include "local_log_def.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+InputEventCircularReader::InputEventCircularReader(size_t numEvents)
+ : mBuffer(new input_event[numEvents * 2]),
+ mBufferEnd(mBuffer + numEvents),
+ mHead(mBuffer),
+ mCurr(mBuffer),
+ mFreeSpace(numEvents)
+{
+}
+
+InputEventCircularReader::~InputEventCircularReader()
+{
+ delete [] mBuffer;
+}
+
+#define INPUT_EVENT_DEBUG (0)
+ssize_t InputEventCircularReader::fill(int fd)
+{
+ size_t numEventsRead = 0;
+ LOGV_IF(INPUT_EVENT_DEBUG,
+ "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
+ if (mFreeSpace) {
+ const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
+ if (nread < 0 || nread % sizeof(input_event)) {
+ //LOGE("Partial event received nread=%d, required=%d",
+ // nread, sizeof(input_event));
+ //LOGE("FD trying to read is: %d");
+ // we got a partial event!!
+ if (INPUT_EVENT_DEBUG) {
+ LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n",
+ __PRETTY_FUNCTION__);
+ LOGV_IF(nread % sizeof(input_event),
+ "DEBUG:%s exit nread %% sizeof(input_event)\n",
+ __PRETTY_FUNCTION__);
+ }
+ return (nread < 0 ? -errno : -EINVAL);
+ }
+
+ numEventsRead = nread / sizeof(input_event);
+ if (numEventsRead) {
+ mHead += numEventsRead;
+ mFreeSpace -= numEventsRead;
+ if (mHead > mBufferEnd) {
+ size_t s = mHead - mBufferEnd;
+ memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
+ mHead = mBuffer + s;
+ }
+ }
+ }
+
+ LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);
+ return numEventsRead;
+}
+
+ssize_t InputEventCircularReader::readEvent(input_event const** events)
+{
+ *events = mCurr;
+ ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
+ return available ? 1 : 0;
+}
+
+void InputEventCircularReader::next()
+{
+ mCurr++;
+ mFreeSpace++;
+ if (mCurr >= mBufferEnd) {
+ mCurr = mBuffer;
+ }
+}
diff --git a/libsensors_iio/InputEventReader.h b/libsensors_iio/InputEventReader.h
index 7cd2ee2..11c4e73 100644
--- a/libsensors_iio/InputEventReader.h
+++ b/libsensors_iio/InputEventReader.h
@@ -22,8 +22,6 @@
#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 7c750c1..e23ecc9 100644
--- a/libsensors_iio/MPLSensor.cpp
+++ b/libsensors_iio/MPLSensor.cpp
@@ -34,7 +34,6 @@
#include <utils/String8.h>
#include <string.h>
#include <linux/input.h>
-#include <utils/Atomic.h>
#include "MPLSensor.h"
#include "MPLSupport.h"
@@ -47,6 +46,7 @@
#include "ml_sysfs_helper.h"
// #define TESTING
+// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */
#ifdef THIRD_PARTY_ACCEL
# warning "Third party accel"
@@ -80,18 +80,11 @@
#define HW_ACCEL_RATE_HZ (1000 / hertz_request)
#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request)
-#define RATE_200HZ 5000000LL
-#define RATE_15HZ 66667000LL
-#define RATE_5HZ 200000000LL
-
static struct sensor_t sSensorList[] =
{
{"MPL Gyroscope", "Invensense", 1,
SENSORS_GYROSCOPE_HANDLE,
SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
- {"MPL Raw Gyroscope", "Invensense", 1,
- SENSORS_RAW_GYROSCOPE_HANDLE,
- SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
{"MPL Accelerometer", "Invensense", 1,
SENSORS_ACCELERATION_HANDLE,
SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}},
@@ -110,12 +103,6 @@
{"MPL Gravity", "Invensense", 1,
SENSORS_GRAVITY_HANDLE,
SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
-
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- {"MPL Screen Orientation", "Invensense ", 1,
- SENSORS_SCREEN_ORIENTATION_HANDLE,
- SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, {}},
-#endif
};
MPLSensor *MPLSensor::gMPLSensor = NULL;
@@ -147,20 +134,17 @@
* MPLSensor class implementation
******************************************************************************/
-// following extended initializer list would only be available with -std=c++11 or -std=gnu+11
-MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
+MPLSensor::MPLSensor(CompassSensor *compass)
: SensorBase(NULL, NULL),
+ mMplSensorInitialized(false),
mNewData(0),
mMasterSensorMask(INV_ALL_SENSORS),
- mLocalSensorMask(0),
+ mLocalSensorMask(ALL_MPL_SENSORS_NP),
mPollTime(-1),
mHaveGoodMpuCal(0),
mGyroAccuracy(0),
mAccelAccuracy(0),
- mCompassAccuracy(0),
mSampleCount(0),
- dmp_orient_fd(-1),
- mDmpOrientationEnabled(0),
mEnabled(0),
mOldEnabledMask(0),
mAccelInputReader(4),
@@ -171,6 +155,8 @@
mAccelScale(2),
mPendingMask(0),
mSensorMask(0),
+ mGyroOrientation{0},
+ mAccelOrientation{0},
mFeatureActiveMask(0) {
VFUNC_LOG;
@@ -185,7 +171,7 @@
mCompassSensor = compass;
LOGV_IF(EXTRA_VERBOSE,
- "HAL:MPLSensor constructor : NUMSENSORS = %d", NUMSENSORS);
+ "HAL:MPLSensor constructor : numSensors = %d", numSensors);
pthread_mutex_init(&mMplMutex, NULL);
pthread_mutex_init(&mHALMutex, NULL);
@@ -198,7 +184,10 @@
#endif
/* setup sysfs paths */
- inv_init_sysfs_attributes();
+ if(inv_init_sysfs_attributes()) {
+ ALOGE("MPLSensor failed to init sysfs attributes");
+ return;
+ }
/* get chip name */
if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
@@ -215,10 +204,11 @@
/* reset driver master enable */
masterEnable(0);
- if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
- /* Load DMP image if capable, ie. MPU6xxx/9xxx */
- loadDMP();
- }
+ /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+ // TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+ loadDMP();
+#endif
/* open temperature fd for temp comp */
LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
@@ -279,11 +269,6 @@
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;
@@ -307,12 +292,11 @@
mHandlers[LinearAccel] = &MPLSensor::laHandler;
mHandlers[Gravity] = &MPLSensor::gravHandler;
mHandlers[Gyro] = &MPLSensor::gyroHandler;
- mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
mHandlers[Accelerometer] = &MPLSensor::accelHandler;
mHandlers[MagneticField] = &MPLSensor::compassHandler;
mHandlers[Orientation] = &MPLSensor::orienHandler;
- for (int i = 0; i < NUMSENSORS; i++) {
+ for (int i = 0; i < numSensors; i++) {
mDelays[i] = 0;
}
@@ -322,35 +306,13 @@
/* setup MPL */
inv_constructor_init();
- /* load calibration file from /data/inv_cal_data.bin */
+ /* load calibration file from /data/cal.bin */
rv = inv_load_calibration();
if(rv == INV_SUCCESS)
LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
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 */
@@ -358,84 +320,67 @@
enableGyro(0);
enableAccel(0);
enableCompass(0);
-
- if ( isLowPowerQuatEnabled() ) {
- enableLPQuaternion(0);
- }
-
onPower(0);
- if (isDmpDisplayOrientationOn()) {
- enableDmpOrientation(isDmpDisplayOrientationOn());
- enableDmpOrientation(!isDmpScreenAutoRotationEnabled()
- && isDmpDisplayOrientationOn());
- }
-
#ifdef INV_PLAYBACK_DBG
logfile = fopen("/data/playback.bin", "wb");
if (logfile)
inv_turn_on_data_logging(logfile);
#endif
+
+ mMplSensorInitialized = true;
}
-void MPLSensor::enable_iio_sysfs()
-{
+
+void MPLSensor::enable_iio_sysfs() {
VFUNC_LOG;
+ int tempFd = 0;
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)",
+ LOGV_IF(SYSFS_VERBOSE,
+ "HAL:sysfs:echo 1 > %s (%lld)",
mpu.in_timestamp_en, getTimestamp());
- // fopen()/open() would both be okay for sysfs access
- // developers could choose what they want
- // with fopen(), benefits from fprintf()/fscanf() would be available
- tempFp = fopen(mpu.in_timestamp_en, "w");
- if (tempFp == NULL) {
+ tempFd = open(mpu.in_timestamp_en, O_RDWR);
+ if(tempFd < 0) {
LOGE("HAL:could not open timestamp enable");
- } else {
- if(fprintf(tempFp, "%d", 1) < 0) {
- LOGE("HAL:could not enable timestamp");
- }
- fclose(tempFp);
+ } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
+ LOGE("HAL:could not enable timestamp");
}
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE,
+ "HAL:sysfs:cat %s (%lld)",
mpu.trigger_name, getTimestamp());
tempFp = fopen(mpu.trigger_name, "r");
if (tempFp == NULL) {
LOGE("HAL:could not open trigger name");
- } else {
- if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
- LOGE("HAL:could not read trigger name");
- }
- fclose(tempFp);
+ } else if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
+ LOGE("HAL:could not read trigger name");
}
+ fclose(tempFp);
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE,
+ "HAL:sysfs:echo %s > %s (%lld)",
iio_trigger_name, mpu.current_trigger, getTimestamp());
tempFp = fopen(mpu.current_trigger, "w");
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");
- }
- fclose(tempFp);
+ } else if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
+ LOGE("HAL:could not write current trigger");
}
+ fclose(tempFp);
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ 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);
+ } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
+ LOGE("HAL:could not write buffer length");
}
+ fclose(tempFp);
inv_get_iio_device_node(iio_device_node);
iio_fd = open(iio_device_node, O_RDONLY);
@@ -483,6 +428,7 @@
return result;
}
+ // TODO: double-check for GED tablet
// result = inv_enable_motion_no_motion();
result = inv_enable_fast_nomot();
if (result) {
@@ -506,13 +452,8 @@
if (result) {
LOG_RESULT_LOCATION(result);
return result;
- } else {
- mFeatureActiveMask |= INV_COMPASS_CAL;
}
- // specify MPL's trust weight, used by compass algorithms
- inv_vector_compass_cal_sensitivity(3);
-
result = inv_enable_compass_bias_w_gyro();
if (result) {
LOG_RESULT_LOCATION(result);
@@ -536,9 +477,6 @@
if (result) {
LOG_RESULT_LOCATION(result);
return result;
- } else {
- // 9x sensor fusion enables Compass fit
- mFeatureActiveMask |= INV_COMPASS_FIT;
}
result = inv_enable_no_gyro_fusion();
@@ -547,6 +485,7 @@
return result;
}
+ // TODO: double-check for GED tablet
result = inv_enable_quat_accuracy_monitor();
if (result) {
LOG_RESULT_LOCATION(result);
@@ -574,15 +513,15 @@
/* accel setup */
orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
- /* use for third party accel input subsystem driver
- inv_set_accel_orientation_and_scale(orient, 1LL << 22);
- */
+ // BMA250
+ //inv_set_accel_orientation_and_scale(orient, 1LL << 22);
+ // MPU6050
inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
/* compass setup */
signed char orientMtx[9];
mCompassSensor->getOrientationMatrix(orientMtx);
- orient =
+ orient =
inv_orientation_matrix_to_scalar(orientMtx);
long sensitivity;
sensitivity = mCompassSensor->getSensitivity();
@@ -594,13 +533,13 @@
int res, fd;
FILE *fptr;
- if (isMpu3050() == true) {
+ if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) {
//DMP support only for MPU6xxx/9xxx currently
return;
}
/* load DMP firmware */
- LOGV_IF(SYSFS_VERBOSE,
+ LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
fd = open(mpu.firmware_loaded, O_RDONLY);
if(fd < 0) {
@@ -633,13 +572,13 @@
FILE *fptr;
// get gyro orientation
- LOGV_IF(SYSFS_VERBOSE,
+ LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
fptr = fopen(mpu.gyro_orient, "r");
if (fptr != NULL) {
int om[9];
fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+ &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
&om[6], &om[7], &om[8]);
fclose(fptr);
@@ -662,13 +601,13 @@
}
// get accel orientation
- LOGV_IF(SYSFS_VERBOSE,
+ LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
fptr = fopen(mpu.accel_orient, "r");
if (fptr != NULL) {
int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+ fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+ &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
&om[6], &om[7], &om[8]);
fclose(fptr);
@@ -698,6 +637,7 @@
/* Close open fds */
if (iio_fd > 0)
close(iio_fd);
+
if( accel_fd > 0 )
close(accel_fd );
if (gyro_temperature_fd > 0)
@@ -705,10 +645,6 @@
if (sysfs_names_ptr)
free(sysfs_names_ptr);
- if (isDmpDisplayOrientationOn()) {
- closeDmpOrientFd();
- }
-
/* Turn off Gyro master enable */
/* A workaround until driver handles it */
/* TODO: Turn off and close all sensors */
@@ -729,13 +665,18 @@
#endif
}
-#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
-#define A_ENABLED ((1 << ID_A) & enabled_sensors)
+#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) // ID_GY = 0
+#define A_ENABLED ((1 << ID_A) & enabled_sensors) // ID_A = 1
+#ifdef INVENSENSE_COMPASS_CAL // ID_M = 2
#define M_ENABLED ((1 << ID_M) & enabled_sensors)
-#define O_ENABLED ((1 << ID_O) & enabled_sensors)
-#define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
-#define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
-#define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
+#else
+// TODO: ID_M = 2 even for 3rd-party solution
+#define M_ENABLED ((1 << ID_M) & enabled_sensors)
+#endif
+#define O_ENABLED ((1 << ID_O) & enabled_sensors) // ID_O = 3
+#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4
+#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5
+#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6
/* TODO: this step is optional, remove? */
int MPLSensor::setGyroInitialState()
@@ -744,7 +685,7 @@
int res = 0;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
int fd = open(mpu.gyro_fifo_rate, O_RDWR);
res = errno;
@@ -765,7 +706,7 @@
return 0;
}
-/* this applies to BMA250 Input Subsystem Driver only */
+/* this applies to BMA250 only */
int MPLSensor::setAccelInitialState()
{
VFUNC_LOG;
@@ -796,7 +737,7 @@
char buf[sizeof(int)+1];
int count, curr_power_state;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.power_state, getTimestamp());
int tempFd = open(mpu.power_state, O_RDWR);
res = errno;
@@ -813,16 +754,16 @@
} else {
sscanf(buf, "%d", &curr_power_state);
}
-
- if (en!=curr_power_state) {
+
+ if (en!=curr_power_state) {
if((res=enable_sysfs_sensor(tempFd, en)) < 0) {
LOGE("HAL:Couldn't write power state");
}
} else {
LOGV_IF(EXTRA_VERBOSE,
- "HAL:Power state already enable/disable curr=%d new=%d",
+ "HAL:Power state already enable/disable curr=%d new=%d",
curr_power_state, en);
- close(tempFd);
+ close(tempFd);
}
}
return res;
@@ -841,32 +782,24 @@
//3. Either Gyro or Accel must be enabled/configured before next step
//4. Enable DMP
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- mpu.firmware_loaded, getTimestamp());
if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
LOGE("HAL:ERR can't get firmware_loaded status");
} else if (status == 1) {
//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");
+ LOGE("HAL:ERR can't write dmp_on");
} else {
- res = 0; //Indicate write successful
+ res= 0; //Indicate write successful
}
//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");
+ LOGE("HAL:ERR can't en/dis DMP interrupt");
}
} else {
- res= 0; //DMP already set as requested
+ res= 0; //DMP already set as requested
}
} else {
LOGE("HAL:ERR No DMP image");
@@ -891,7 +824,7 @@
mFeatureActiveMask &= ~INV_DMP_QUATERNION;
LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
} else {
- if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
+ if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
LOGE("HAL:ERR can't enable LP Quaternion");
} else {
mFeatureActiveMask |= INV_DMP_QUATERNION;
@@ -906,65 +839,33 @@
int res= 0;
VFUNC_LOG;
- // Enable DMP quaternion
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, mpu.quaternion_on, getTimestamp());
+ //Enable DMP quaternion
if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
LOGE("HAL:ERR can't write DMP quaternion_on");
- res = -1; //Indicate an err
- }
+ res= -1; //Indicate an err
+ }
if (!en) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 0, mpu.in_quat_r_en, getTimestamp());
- if (write_sysfs_int(mpu.in_quat_r_en, 0) < 0) {
- LOGE("HAL:ERR write in_quat_r_en");
- }
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 0, mpu.in_quat_x_en, getTimestamp());
- if (write_sysfs_int(mpu.in_quat_x_en, 0) < 0) {
- LOGE("HAL:ERR write in_quat_x_en");
- }
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 0, mpu.in_quat_y_en, getTimestamp());
- if (write_sysfs_int(mpu.in_quat_y_en, 0) < 0) {
- LOGE("HAL:ERR write in_quat_y_en");
- }
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 0, mpu.in_quat_z_en, getTimestamp());
-
- if (write_sysfs_int(mpu.in_quat_z_en, 0) < 0) {
- LOGE("HAL:ERR write in_quat_z_en");
- }
-
LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
inv_quaternion_sensor_was_turned_off();
} else {
LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 1, mpu.in_quat_r_en, getTimestamp());
if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) {
LOGE("HAL:ERR write in_quat_r_en");
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 1, mpu.in_quat_x_en, getTimestamp());
if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) {
LOGE("HAL:ERR write in_quat_x_en");
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 1, mpu.in_quat_y_en, getTimestamp());
if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) {
LOGE("HAL:ERR write in_quat_y_en");
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- 1, mpu.in_quat_z_en, getTimestamp());
if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) {
- LOGE("HAL:ERR write in_quat_z_en");
+ LOGE("HAL:ERR write in_quat_z_en");
}
}
return res;
+
}
int MPLSensor::enableTap(int en)
@@ -994,7 +895,7 @@
int res = 0;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.chip_enable, getTimestamp());
int tempFd = open(mpu.chip_enable, O_RDWR);
res = errno;
@@ -1014,7 +915,7 @@
int res = 0;
/* need to also turn on/off the master enable */
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_enable, getTimestamp());
int tempFd = open(mpu.gyro_enable, O_RDWR);
res = errno;
@@ -1029,7 +930,7 @@
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)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_x_fifo_enable, getTimestamp());
tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR);
res = errno;
@@ -1040,7 +941,7 @@
mpu.gyro_x_fifo_enable, strerror(res), res);
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_y_fifo_enable, getTimestamp());
tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR);
res = errno;
@@ -1051,7 +952,7 @@
mpu.gyro_y_fifo_enable, strerror(res), res);
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_z_fifo_enable, getTimestamp());
tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR);
res = errno;
@@ -1072,23 +973,23 @@
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());
- int tempFd = open(mpu.accel_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.accel_enable, strerror(res), 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());
+ int tempFd = open(mpu.accel_enable, O_RDWR);
+ res = errno;
+ if (tempFd > 0) {
+ res = enable_sysfs_sensor(tempFd, en);
+ } else {
+ LOGE("HAL:open of %s failed with '%s' (%d)",
+ mpu.accel_enable, strerror(res), res);
+ }
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)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.accel_x_fifo_enable, getTimestamp());
tempFd = open(mpu.accel_x_fifo_enable, O_RDWR);
res = errno;
@@ -1099,7 +1000,7 @@
mpu.accel_x_fifo_enable, strerror(res), res);
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.accel_y_fifo_enable, getTimestamp());
tempFd = open(mpu.accel_y_fifo_enable, O_RDWR);
res = errno;
@@ -1110,7 +1011,7 @@
mpu.accel_y_fifo_enable, strerror(res), res);
}
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.accel_z_fifo_enable, getTimestamp());
tempFd = open(mpu.accel_z_fifo_enable, O_RDWR);
res = errno;
@@ -1123,10 +1024,10 @@
}
if (!en && USE_THIRD_PARTY_ACCEL == 0) {
- }
+ }
if(USE_THIRD_PARTY_ACCEL == 1 && en) {
- //setAccelInitialState();// BMA250
+ setAccelInitialState(); // BMA250
}
return res;
}
@@ -1139,7 +1040,7 @@
if (en == 0) {
LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
inv_compass_was_turned_off();
- }
+ }
return res;
}
@@ -1194,20 +1095,18 @@
inv_error_t res = -1;
int on = 1;
int off = 0;
- int cal_stored = 0;
// Sequence to enable or disable a sensor
- // 1. enable Power state
+ // 1. enable Power state
// 2. reset master enable (=0)
// 3. enable or disable a sensor
// 4. set master enable (=1)
- if (isLowPowerQuatEnabled() ||
- changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
- (mCompassSensor->isIntegrated() << MagneticField))) {
+ if (changed & ((1 << Gyro) | (1 << Accelerometer) |
+ (mCompassSensor->isIntegrated() << MagneticField))) {
/* ensure power state is on */
onPower(1);
-
+
/* reset master enable */
res = masterEnable(0);
if(res < 0) {
@@ -1217,45 +1116,35 @@
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
- if (changed & ((1 << Gyro) | (1 << RawGyro))) {
- if (sensors & INV_THREE_AXIS_GYRO) {
+ if (changed & (1 << Gyro)) {
+ 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) {
+ } else if((sensors & INV_THREE_AXIS_GYRO) == 0) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
res = enableGyro(off);
if(res < 0) {
return res;
}
-
- if (!cal_stored) {
- storeCalibration();
- cal_stored = 1;
- }
}
}
if (changed & (1 << Accelerometer)) {
- if (sensors & INV_THREE_AXIS_ACCEL) {
+ 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) {
+ } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
res = enableAccel(off);
if(res < 0) {
return res;
}
-
- if (!cal_stored) {
- storeCalibration();
- cal_stored = 1;
- }
}
}
@@ -1273,121 +1162,51 @@
if(res < 0) {
return res;
}
-
- if (!cal_stored) {
- storeCalibration();
- cal_stored = 1;
- }
}
}
- if ( isLowPowerQuatEnabled() ) {
- // Enable LP Quat
- if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
- (1 << LinearAccel) | (1 << Gravity)))) {
- if (!(changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
- (mCompassSensor->isIntegrated() << MagneticField)))) {
- /* ensure power state is on */
- onPower(1);
- /* reset master enable */
- res = masterEnable(0);
- if(res < 0) {
- return res;
- }
- }
- if (!checkLPQuaternion()) {
- enableLPQuaternion(1);
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
- }
- } else if (checkLPQuaternion()) {
- enableLPQuaternion(0);
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+ // Enable LP Quat
+ if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+ (1 << LinearAccel) | (1 << Gravity)))) {
+ if (!checkLPQuaternion()) {
+ enableLPQuaternion(1);
+ } else {
+ LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
}
+ } else if (checkLPQuaternion()) {
+ enableLPQuaternion(0);
}
+#endif
- if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+ /*
+ if sensor & THREE_AXIS_GYRO
+ enable = 1
+ if sensor & THREE_AXIS_ACCEL
+ enable = 1
+ if compass_on_secondary
+ if sensor & THREE_AXIS_COMPASS
+ enable = 1
+ else
+ enable = 0
+ */
+ if (changed & ((1 << Gyro) | (1 << Accelerometer) |
(mCompassSensor->isIntegrated() << MagneticField))) {
- if (sensors &
- (INV_THREE_AXIS_GYRO
- | INV_THREE_AXIS_ACCEL
+ if (sensors &
+ (INV_THREE_AXIS_GYRO
+ | INV_THREE_AXIS_ACCEL
| (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
- 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;
- }
- }
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- // enable DMP
- onDMP(1);
-
- res = enableAccel(on);
- if(res < 0) {
- return res;
- }
-
- if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
- res = turnOffAccelFifo();
- }
- if(res < 0) {
- return res;
- }
- }
-
res = masterEnable(1);
if(res < 0) {
return res;
}
} else { // all sensors idle -> reduce power
- 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);
- if(res < 0) {
- return res;
- }
- if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
- res = turnOffAccelFifo();
- }
- if(res < 0) {
- return res;
- }
- res = masterEnable(1);
- if(res < 0) {
- return res;
- }
+ res = onPower(0);
+ if(res < 0) {
+ return res;
}
- else {
- res = onPower(0);
- if(res < 0) {
- return res;
- }
- }
-
- if (!cal_stored) {
- storeCalibration();
- cal_stored = 1;
- }
- }
- } else if ((changed & ((!mCompassSensor->isIntegrated()) << MagneticField)) &&
- !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
- | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
- if (!cal_stored) {
storeCalibration();
- cal_stored = 1;
}
}
@@ -1397,7 +1216,7 @@
/* Store calibration file */
void MPLSensor::storeCalibration()
{
- if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
+ if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) {
int res = inv_store_calibration();
if (res) {
LOGE("HAL:Cannot store calibration on file");
@@ -1425,16 +1244,6 @@
return update;
}
-int MPLSensor::rawGyroHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int update;
- update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &s->gyro.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;
-}
-
int MPLSensor::accelHandler(sensors_event_t* s)
{
VHANDLER_LOG;
@@ -1442,7 +1251,7 @@
update = inv_get_sensor_type_accelerometer(
s->acceleration.v, &s->acceleration.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->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
s->timestamp, update);
mAccelAccuracy = s->acceleration.status;
return update;
@@ -1456,7 +1265,6 @@
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;
}
@@ -1509,17 +1317,9 @@
VFUNC_LOG;
android::String8 sname;
- int what = -1, err = 0;
+ int what = -1;
switch (handle) {
- case ID_SO:
- sname = "Screen Orientation";
- LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
- (mDmpOrientationEnabled? "en": "dis"),
- (en? "en" : "dis"));
- enableDmpOrientation(en && isDmpDisplayOrientationOn());
- mDmpOrientationEnabled = en;
- return 0;
case ID_A:
what = Accelerometer;
sname = "Accelerometer";
@@ -1536,10 +1336,6 @@
what = Gyro;
sname = "Gyro";
break;
- case ID_RG:
- what = RawGyro;
- sname = "RawGyro";
- break;
case ID_GR:
what = Gravity;
sname = "Gravity";
@@ -1558,10 +1354,11 @@
break;
}
- if (uint32_t(what) >= NUMSENSORS)
+ if (uint32_t(what) >= numSensors)
return -EINVAL;
int newState = en ? 1 : 0;
+ int err = 0;
unsigned long sen_mask;
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
@@ -1570,6 +1367,7 @@
LOGV_IF(PROCESS_VERBOSE,
"HAL:%s sensor state change what=%d", sname.string(), what);
+ // TODO: disabled for GED tablet
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
@@ -1591,7 +1389,6 @@
switch (what) {
case Gyro:
- case RawGyro:
case Accelerometer:
case MagneticField:
if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
@@ -1625,7 +1422,7 @@
// pthread_mutex_unlock(&mHALMutex);
#ifdef INV_PLAYBACK_DBG
- /* apparently the logging needs to be go through this sequence
+ /* apparently the logging needs to be go through this sequence
to properly flush the log file */
inv_turn_off_data_logging();
fclose(logfile);
@@ -1645,46 +1442,41 @@
int what = -1;
switch (handle) {
- 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;
+ 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_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;
}
+// TODO: disabled for GED tablet
#if 0
// skip the 1st call for enalbing sensors called by ICS/JB sensor service
static int counter_delay = 0;
@@ -1700,7 +1492,7 @@
}
#endif
- if (uint32_t(what) >= NUMSENSORS)
+ if (uint32_t(what) >= numSensors)
return -EINVAL;
if (ns < 0)
@@ -1708,6 +1500,7 @@
LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
+ // TODO: for GED tablet
// limit all rates to reasonable ones */
/*
if (ns < 10000000LL) {
@@ -1723,7 +1516,6 @@
switch (what) {
case Gyro:
- case RawGyro:
case Accelerometer:
for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
i++) {
@@ -1737,7 +1529,6 @@
case MagneticField:
if (mCompassSensor->isIntegrated() &&
(((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
- ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
return 0;
@@ -1748,12 +1539,7 @@
case RotationVector:
case LinearAccel:
case Gravity:
- if ( isLowPowerQuatEnabled() ) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
- break;
- }
-
- for (int i = 0; i < NUMSENSORS; i++) {
+ for (int i = 0; i < numSensors; i++) {
if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
return 0;
@@ -1762,6 +1548,7 @@
break;
}
+ // TODO: disabled for GED tablet
// pthread_mutex_lock(&mHALMutex);
int res = update_delay();
// pthread_mutex_unlock(&mHALMutex);
@@ -1775,31 +1562,33 @@
int64_t got;
if (mEnabled) {
- int64_t wanted = 1000000000;
- int64_t wanted_3rd_party_sensor = 1000000000;
+ uint64_t wanted = -1LLU;
+ uint64_t wanted_ec = -1LLU;
- // Sequence to change sensor's FIFO rate
- // 1. enable Power state
+ // Sequence to change sensor's FIFO rate
+ // 1. enable Power state
// 2. reset master enable
// 3. Update delay
// 4. set master enable
+ // TODO: unnecessary for IIO
// ensure power on
- onPower(1);
+ // onPower(1);
+ // TODO: unnecessary for IIO
// reset master enable
- masterEnable(0);
+ // masterEnable(0);
/* search the minimum delay requested across all enabled sensors */
- for (int i = 0; i < NUMSENSORS; i++) {
+ for (int i = 0; i < numSensors; i++) {
if (mEnabled & (1 << i)) {
- int64_t ns = mDelays[i];
+ uint64_t ns = mDelays[i];
wanted = wanted < ns ? wanted : ns;
}
}
- // same delay for 3rd party Accel or Compass
- wanted_3rd_party_sensor = wanted;
+ // same delay for ext compass
+ wanted_ec = wanted;
/* mpl rate in us in future maybe different for
gyro vs compass vs accel */
@@ -1818,6 +1607,25 @@
inv_set_accel_sample_rate(mplAccelRate);
inv_set_compass_sample_rate(mplCompassRate);
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+ // Set LP Quaternion sample rate if enabled
+ if (checkLPQuaternion()) {
+ // TODO: need to verify this part for LPQ
+ if (wanted < 5000000LL) {
+ enableLPQuaternion(0);
+ } else {
+ inv_set_quat_sample_rate(rateInus);
+ // set DMP output rate to FIFO
+ write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted);
+
+ //DMP running rate must be @ 200Hz
+ wanted= 5000000LL;
+ LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus);
+ }
+ }
+#endif
+
/* TODO: Test 200Hz */
// inv_set_gyro_sample_rate(5000);
LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
@@ -1826,30 +1634,12 @@
int enabled_sensors = mEnabled;
int tempFd = -1;
- 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()) {
- if (wanted < RATE_200HZ) {
- enableLPQuaternion(0);
- } else {
- inv_set_quat_sample_rate(rateInus);
- setDMPrate= 1;
- }
- }
-
- if (checkDMPOrientation() || setDMPrate==1) {
- getDmpRate(&wanted);
- }
- }
-
- int64_t tempRate = wanted;
+ if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
+ uint64_t tempRate = wanted;
LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
//nsToHz
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / tempRate, mpu.gyro_fifo_rate,
+ 1000000000.f / tempRate, mpu.gyro_fifo_rate,
getTimestamp());
tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
@@ -1860,16 +1650,15 @@
//nsToHz (BMA250)
if(USE_THIRD_PARTY_ACCEL == 1) {
LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
- wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
+ wanted / 1000000L, mpu.accel_fifo_rate,
getTimestamp());
tempFd = open(mpu.accel_fifo_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
+ res = write_attribute_sensor(tempFd, wanted / 1000000L);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
}
if (!mCompassSensor->isIntegrated()) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
- mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
+ mCompassSensor->setDelay(ID_M, wanted_ec);
got = mCompassSensor->getDelay(ID_M);
inv_set_compass_sample_rate(got / 1000);
}
@@ -1890,14 +1679,7 @@
} else {
if (GY_ENABLED) {
- wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
- (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
- (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- getDmpRate(&wanted);
- }
-
+ wanted = mDelays[Gyro];
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
@@ -1908,30 +1690,17 @@
if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
wanted = mDelays[Gyro];
- }
- else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
- wanted = mDelays[RawGyro];
-
} else {
wanted = mDelays[Accelerometer];
}
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- getDmpRate(&wanted);
- }
-
/* TODO: use function pointers to calculate delay value specific to vendor */
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);
- if(USE_THIRD_PARTY_ACCEL == 1) {
- //BMA250 in ms
- res = write_attribute_sensor(tempFd, wanted / 1000000L);
- }
- else {
- //MPUxxxx in hz
- res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
- }
+ //BMA250 in ms
+ //res = write_attribute_sensor(tempFd, wanted / 1000000L);
+ //MPU6050 in hz
+ res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
}
@@ -1942,20 +1711,12 @@
} else {
if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
wanted = mDelays[Gyro];
- }
- else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
- wanted = mDelays[RawGyro];
- } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
+ } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
wanted = mDelays[Accelerometer];
} else {
wanted = mDelays[MagneticField];
}
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- getDmpRate(&wanted);
- }
}
-
mCompassSensor->setDelay(ID_M, wanted);
got = mCompassSensor->getDelay(ID_M);
inv_set_compass_sample_rate(got / 1000);
@@ -1963,27 +1724,44 @@
}
+ /*
+ if sensor & THREE_AXIS_GYRO
+ enable = 1
+ if sensor & THREE_AXIS_ACCEL
+ enable = 1
+ if compass_on_secondary
+ if sensor & THREE_AXIS_COMPASS
+ enable = 1
+ else
+ enable = 0
+ */
unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
- if (sensors &
- (INV_THREE_AXIS_GYRO
- | INV_THREE_AXIS_ACCEL
+ if (sensors &
+ (INV_THREE_AXIS_GYRO
+ | INV_THREE_AXIS_ACCEL
| (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+// TODO: unnecessary for IIO
+#if 0
res = masterEnable(1);
if(res < 0) {
return res;
}
+#endif
} else { // all sensors idle -> reduce power
+// TODO: unnecessary for IIO
+#if 0
res = onPower(0);
if(res < 0) {
return res;
}
+#endif
}
}
return res;
}
-/* For Third Party Accel Input Subsystem Drivers only */
+/* use for third party accel */
int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
{
VHANDLER_LOG;
@@ -2026,12 +1804,15 @@
mAccelInputReader.next();
}
+ LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived);
+
return numEventReceived;
}
-/**
+/**
* Should be called after reading at least one of gyro
- * compass or accel data. (Also okay for handling all of them).
+ * compass or accel data. You should only read 1 sample of
+ * data and call this.
* @returns 0, if successful, error number if not.
*/
int MPLSensor::executeOnData(sensors_event_t* data, int count)
@@ -2060,7 +1841,7 @@
}
// load up virtual sensors
- for (int i = 0; i < NUMSENSORS; i++) {
+ for (int i = 0; i < numSensors; i++) {
int update;
if (mEnabled & (1 << i)) {
update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
@@ -2077,11 +1858,10 @@
return numEventReceived;
}
-// collect data for MPL (but NOT sensor service currently), from driver layer
int MPLSensor::readEvents(sensors_event_t *data, int count) {
- int lp_quaternion_on = 0, nbyte;
+ int lp_quaternion_on, nbyte;
int i, nb, mask = 0, numEventReceived = 0,
sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1: 0) +
@@ -2090,23 +1870,24 @@
nbyte= (8 * sensors + 8) * 1;
- if (isLowPowerQuatEnabled()) {
- lp_quaternion_on = checkLPQuaternion();
- if (lp_quaternion_on == 1) {
- nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data
- }
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+ lp_quaternion_on = checkLPQuaternion();
+ if (lp_quaternion_on == 1) {
+ nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data
}
+#endif
+ // TODO: disabled for GED tablet
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
- ssize_t rsize = read(iio_fd, rdata, nbyte);
+ size_t rsize = read(iio_fd, rdata, nbyte);
if (sensors == 0) {
// read(iio_fd, rdata, nbyte);
- read(iio_fd, rdata, IIO_BUFFER_LENGTH);
+ read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH);
}
-
-#ifdef TESTING
+/*
LOGI("get one sample of IIO data with size: %d", rsize);
LOGI("sensors: %d", sensors);
@@ -2126,20 +1907,27 @@
((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
*((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
-#endif
+*/
+ // TODO: need to verify this for LPQ
if (rsize < (nbyte - 8)) {
LOGE("HAL:ERR Full data packet was not read");
// return -1;
}
- if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
-
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+ if (lp_quaternion_on == 1) {
for (i=0; i< 4; i++) {
mCachedQuaternionData[i]= *(long*)rdata;
rdata += sizeof(long);
}
}
+/*
+ LOGV("HAL:rdata= %x sensors= %d lp_q_on= %d nbyte= %d rsize= %d",
+ rdata, sensors, lp_quaternion_on, nbyte, rsize); //tbd
+*/
+#endif
for (i = 0; i < 3; i++) {
if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
@@ -2154,11 +1942,11 @@
}
}
- mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
+ mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
+ ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0));
if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
(mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
- mask |= 1 << MagneticField;
+ mask |= 4;
}
mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
@@ -2166,61 +1954,58 @@
mCompassTimestamp = mSensorTimestamp;
}
- if (mask & (1 << Gyro)) {
- // send down temperature every 0.5 seconds
- // with timestamp measured in "driver" layer
- if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
- mTempCurrentTime = mSensorTimestamp;
- long long temperature[2];
- if(inv_read_temperature(temperature) == 0) {
- LOGV_IF(INPUT_DATA,
- "HAL:inv_read_temperature = %lld, timestamp= %lld",
- temperature[0], temperature[1]);
- inv_build_temp(temperature[0], temperature[1]);
- }
-#ifdef TESTING
- long bias[3], temp, temp_slope[3];
- inv_get_gyro_bias(bias, &temp);
- inv_get_gyro_ts(temp_slope);
-
- LOGI("T: %.3f "
- "GB: %+13f %+13f %+13f "
- "TS: %+13f %+13f %+13f "
- "\n",
- (float)temperature[0] / 65536.f,
- (float)bias[0] / 65536.f / 16.384f,
- (float)bias[1] / 65536.f / 16.384f,
- (float)bias[2] / 65536.f / 16.384f,
- temp_slope[0] / 65536.f,
- temp_slope[1] / 65536.f,
- temp_slope[2] / 65536.f);
-#endif
+ // send down temperature every 0.5 seconds
+ if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+ mTempCurrentTime = mSensorTimestamp;
+ long long temperature[2];
+ if(inv_read_temperature(temperature) == 0) {
+ LOGV_IF(INPUT_DATA,
+ "HAL:inv_read_temperature = %lld, timestamp= %lld",
+ temperature[0], temperature[1]);
+ inv_build_temp(temperature[0], temperature[1]);
}
+#ifdef TESTING
+ long bias[3], temp, temp_slope[3];
+ inv_get_gyro_bias(bias, &temp);
+ inv_get_gyro_ts(temp_slope);
+
+ LOGI("T: %.3f "
+ "GB: %+13f %+13f %+13f "
+ "TS: %+13f %+13f %+13f "
+ "\n",
+ (float)temperature[0] / 65536.f,
+ (float)bias[0] / 65536.f / 16.384f,
+ (float)bias[1] / 65536.f / 16.384f,
+ (float)bias[2] / 65536.f / 16.384f,
+ temp_slope[0] / 65536.f,
+ temp_slope[1] / 65536.f,
+ temp_slope[2] / 65536.f);
+#endif
+ }
+ if (mask & 1) {
mPendingMask |= 1 << Gyro;
- mPendingMask |= 1 << RawGyro;
-
if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
inv_build_gyro(mCachedGyroData, mSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
- mCachedGyroData[0], mCachedGyroData[1],
+ mCachedGyroData[0], mCachedGyroData[1],
mCachedGyroData[2], mSensorTimestamp);
}
}
- if (mask & (1 << Accelerometer)) {
+ if (mask & 2) {
mPendingMask |= 1 << Accelerometer;
if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
- mCachedAccelData[0], mCachedAccelData[1],
+ mCachedAccelData[0], mCachedAccelData[1],
mCachedAccelData[2], mSensorTimestamp);
}
}
- if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
+ if ((mask & 4) && mCompassSensor->isIntegrated()) {
int status = 0;
if (mCompassSensor->providesCalibration()) {
status = mCompassSensor->getAccuracy();
@@ -2229,19 +2014,21 @@
if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
+ LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+ mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
}
}
- if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
-
+// TODO: disabled for GED tablet
+#ifdef ENABLE_LP_QUAT_FEAT
+ if (lp_quaternion_on == 1) {
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",
- mCachedQuaternionData[0], mCachedQuaternionData[1],
+ LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld - %d",
+ mCachedQuaternionData[0], mCachedQuaternionData[1],
mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
}
+#endif
// pthread_mutex_unlock(&mMplMutex);
// pthread_mutex_unlock(&mHALMutex);
@@ -2254,20 +2041,19 @@
{
VHANDLER_LOG;
+ if (count < 1)
+ return -EINVAL;
+
int numEventReceived = 0;
int done = 0;
int nb;
+ // TODO: disabled for GED tablet
+ // TODO: for AMI306
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
-#ifdef COMPASS_YAS53x
- if (mCompassSensor->checkCoilsReset() == 1) {
- //Reset relevant compass settings
- resetCompass();
- }
-#endif
if (done > 0) {
int status = 0;
if (mCompassSensor->providesCalibration()) {
@@ -2277,8 +2063,8 @@
if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
+ LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+ mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
}
}
@@ -2289,27 +2075,6 @@
return numEventReceived;
}
-#ifdef COMPASS_YAS53x
-int MPLSensor::resetCompass()
-{
- VFUNC_LOG;
-
- //Reset compass cal if enabled
- if (mFeatureActiveMask & INV_COMPASS_CAL) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
- inv_init_vector_compass_cal();
- }
-
- //Reset compass fit if enabled
- if (mFeatureActiveMask & INV_COMPASS_FIT) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
- inv_init_compass_fit();
- }
-
- return 0;
-}
-#endif
-
int MPLSensor::getFd() const
{
VFUNC_LOG;
@@ -2332,206 +2097,6 @@
return fd;
}
-int MPLSensor::turnOffAccelFifo() {
- int i, res, tempFd;
- char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
- 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());
- tempFd = open(accel_fifo_enable[i], O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, 0);
- if (res < 0) {
- return res;
- }
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- accel_fifo_enable[i], strerror(res), res);
- return res;
- }
- }
-
- return res;
-}
-
-int MPLSensor::enableDmpOrientation(int en)
-{
- VFUNC_LOG;
- int res = 0;
- int enabled_sensors = mEnabled;
-
- if ( isMpu3050() )
- return res;
-
- // on power if not already On
- res = onPower(1);
- // reset master enable
- res = masterEnable(0);
-
- if (en == 1) {
- //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
- }
-
- // open DMP Orient Fd
- res = openDmpOrientFd();
-
- // 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());
- 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());
- 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
- res = enableAccel(1);
-
- // disable accel FIFO
- if (!A_ENABLED) {
- res = turnOffAccelFifo();
- }
-
- mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
- } else {
- // disable DMP
- res = onDMP(0);
- // disable accel engine
- if (!A_ENABLED) {
- res = enableAccel(0);
- }
- }
-
- res = masterEnable(1);
- return res;
-}
-
-int MPLSensor::openDmpOrientFd()
-{
- VFUNC_LOG;
-
- if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
- return -1;
- }
-
- dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
- if (dmp_orient_fd < 0) {
- LOGE("HAL:ERR couldn't open dmpOrient node");
- return -1;
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
- return 0;
- }
-}
-
-int MPLSensor::closeDmpOrientFd()
-{
- VFUNC_LOG;
- if (dmp_orient_fd >= 0)
- close(dmp_orient_fd);
- return 0;
-}
-
-int MPLSensor::dmpOrientHandler(int orient)
-{
- VFUNC_LOG;
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
- return 0;
-}
-
-int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
- VFUNC_LOG;
-
- char dummy[4];
- int screen_orientation = 0;
- FILE *fp;
-
- fp = fopen(mpu.event_display_orientation, "r");
- if (fp == NULL) {
- LOGE("HAL:cannot open event_display_orientation");
- return 0;
- }
- fscanf(fp, "%d\n", &screen_orientation);
- fclose(fp);
-
- int numEventReceived = 0;
-
- if (mDmpOrientationEnabled && count > 0) {
- sensors_event_t temp;
-
- temp.version = sizeof(sensors_event_t);
- temp.sensor = ID_SO;
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
- temp.screen_orientation = screen_orientation;
-#endif
- struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC, &ts);
- temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
-
- *data++ = temp;
- count--;
- numEventReceived++;
- }
-
- // read dummy data per driver's request
- dmpOrientHandler(screen_orientation);
- read(dmp_orient_fd, dummy, 4);
-
- return numEventReceived;
-}
-
-int MPLSensor::getDmpOrientFd()
-{
- VFUNC_LOG;
-
- LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
- return dmp_orient_fd;
-
-}
-
-int MPLSensor::checkDMPOrientation()
-{
- VFUNC_LOG;
- return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
-}
-
-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);
-
- //DMP running rate must be @ 200Hz
- *wanted= RATE_200HZ;
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
- }
- return 0;
-}
-
int MPLSensor::getPollTime()
{
VHANDLER_LOG;
@@ -2629,7 +2194,7 @@
return -1;
}
- LOGV_IF(ENG_VERBOSE,
+ LOGV_IF(ENG_VERBOSE,
"HAL:temperature raw = %ld, timestamp = %lld, count = %d",
raw, timestamp, count);
data[0] = raw;
@@ -2687,39 +2252,41 @@
count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
if(count) {
/* scale appropriately for MPL */
- LOGV_IF(ENG_VERBOSE,
- "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
+ LOGV_IF(ENG_VERBOSE,
+ "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
atol(x), atol(y), atol(z));
data[0] = (long)(atol(x) / 10000 * (1L << 16));
data[1] = (long)(atol(y) / 10000 * (1L << 16));
data[2] = (long)(atol(z) / 10000 * (1L << 16));
- LOGV_IF(ENG_VERBOSE,
- "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
+ LOGV_IF(ENG_VERBOSE,
+ "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
data[0], data[1], data[2]);
}
return 0;
}
+
/** fill in the sensor list based on which sensors are configured.
* return the number of configured sensors.
* parameter list must point to a memory region of at least 7*sizeof(sensor_t)
* parameter len gives the length of the buffer pointed to by list
*/
+
int MPLSensor::populateSensorList(struct sensor_t *list, int len)
{
VFUNC_LOG;
int numsensors;
- if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
+ if(len < (int)(7 * sizeof(sensor_t))) {
LOGE("HAL:sensor list too small, not populating.");
- return -(sizeof(sSensorList) / sizeof(sensor_t));
+ return 0;
}
/* fill in the base values */
- memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
+ memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
/* first add gyro, accel and compass to the list */
@@ -2734,7 +2301,7 @@
mCompassSensor->fillList(&list[MagneticField]);
if(1) {
- numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
+ numsensors = 7;
/* all sensors will be added to the list
fill in orientation values */
fillOrientation(list);
@@ -2768,13 +2335,10 @@
list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6050_POWER;
- list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
- return;
- } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
- list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
- list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
- list[Accelerometer].power = ACCEL_MPU6500_POWER;
- list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
+
+ // TODO: for GED tablet
+ // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+ list[Accelerometer].minDelay = 5000;
return;
} else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
@@ -2783,7 +2347,7 @@
list[Accelerometer].power = ACCEL_MPU9150_POWER;
list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
return;
- } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
+ } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA250_POWER;
@@ -2814,12 +2378,10 @@
list[Gyro].maxRange = GYRO_MPU6050_RANGE;
list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
list[Gyro].power = GYRO_MPU6050_POWER;
- list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
- } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
- list[Gyro].maxRange = GYRO_MPU6500_RANGE;
- list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
- list[Gyro].power = GYRO_MPU6500_POWER;
- list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
+
+ // TODO: for GED tablet
+ // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+ list[Gyro].minDelay = 5000;
} else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
list[Gyro].maxRange = GYRO_MPU9150_RANGE;
list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
@@ -2833,12 +2395,6 @@
list[Gyro].power = GYRO_MPU3050_POWER;
list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
}
-
- list[RawGyro].maxRange = list[Gyro].maxRange;
- list[RawGyro].resolution = list[Gyro].resolution;
- list[RawGyro].power = list[Gyro].power;
- list[RawGyro].minDelay = list[Gyro].minDelay;
-
return;
}
@@ -2848,11 +2404,14 @@
VFUNC_LOG;
/* compute power on the fly */
- list[RotationVector].power = list[Gyro].power +
+ list[RotationVector].power = list[Gyro].power +
list[Accelerometer].power +
list[MagneticField].power;
list[RotationVector].resolution = .00001;
list[RotationVector].maxRange = 1.0;
+
+ // TODO: for GED tablet
+ // list[RotationVector].minDelay = 10000;
list[RotationVector].minDelay = 5000;
return;
@@ -2867,6 +2426,9 @@
list[MagneticField].power;
list[Orientation].resolution = .00001;
list[Orientation].maxRange = 360.0;
+
+ // TODO: for GED tablet
+ // list[Orientation].minDelay = 10000;
list[Orientation].minDelay = 5000;
return;
@@ -2881,6 +2443,9 @@
list[MagneticField].power;
list[Gravity].resolution = .00001;
list[Gravity].maxRange = 9.81;
+
+ // TODO: for GED tablet
+ // list[Gravity].minDelay = 10000;
list[Gravity].minDelay = 5000;
return;
@@ -2895,6 +2460,9 @@
list[MagneticField].power;
list[LinearAccel].resolution = list[Accelerometer].resolution;
list[LinearAccel].maxRange = list[Accelerometer].maxRange;
+
+ // TODO: for GED tablet
+ // list[LinearAccel].minDelay = 10000;
list[LinearAccel].minDelay = 5000;
return;
@@ -2910,7 +2478,13 @@
char **dptr;
int num;
- sysfs_names_ptr =
+ // 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) {
@@ -2924,9 +2498,6 @@
return -1;
}
- // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
- // inv_get_sysfs_abs_path(sysfs_path);
- inv_get_sysfs_path(sysfs_path);
inv_get_iio_trigger_path(iio_trigger_path);
sprintf(mpu.key, "%s%s", sysfs_path, "/key");
@@ -2941,7 +2512,6 @@
sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
- sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
@@ -2956,58 +2526,39 @@
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, "/enable");
+ sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate");
+ 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, "/NA");
+#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_orient, "%s%s", sysfs_path, "/accl_matrix");
-
-#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");
-#endif
-
sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
+ // 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.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
- sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
- sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
-
- return 0;
-}
-
-bool MPLSensor::isMpu3050()
-{
- if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
- return true;
- else
- return false;
-}
-
-int MPLSensor::isLowPowerQuatEnabled()
-{
-#ifdef ENABLE_LP_QUAT_FEAT
- if (isMpu3050())
- return 0;
- return 1;
-#else
- return 0;
+#if 0
+ // test print sysfs paths
+ dptr = (char**)&mpu;
+ for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
+ LOGE("HAL:sysfs path: %s", *dptr++);
+ }
#endif
-}
-
-int MPLSensor::isDmpDisplayOrientationOn()
-{
-#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
- if (isMpu3050())
- return 0;
- return 1;
-#else
return 0;
-#endif
}
diff --git a/libsensors_iio/MPLSensor.h b/libsensors_iio/MPLSensor.h
index 3f6bb5b..3ef1ba3 100644
--- a/libsensors_iio/MPLSensor.h
+++ b/libsensors_iio/MPLSensor.h
@@ -1,345 +1,288 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_MPL_SENSOR_H
-#define ANDROID_MPL_SENSOR_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <poll.h>
-#include <utils/Vector.h>
-#include <utils/KeyedVector.h>
-#include "sensors.h"
-#include "SensorBase.h"
-#include "InputEventReader.h"
-
-#ifdef INVENSENSE_COMPASS_CAL
-
-#ifdef COMPASS_YAS53x
-#warning "unified HAL for YAS53x"
-#include "CompassSensor.IIO.primary.h"
-#elif COMPASS_AMI306
-#warning "unified HAL for AMI306"
-#include "CompassSensor.IIO.primary.h"
-#else
-#warning "unified HAL for MPU9150"
-#include "CompassSensor.IIO.9150.h"
-#endif
-#else
-#warning "unified HAL for AKM"
-#include "CompassSensor.AKM.h"
-#endif
-
-/*****************************************************************************/
-/* Sensors Enable/Disable Mask
- *****************************************************************************/
-#define MAX_CHIP_ID_LEN (20)
-
-#define INV_THREE_AXIS_GYRO (0x000F)
-#define INV_THREE_AXIS_ACCEL (0x0070)
-#define INV_THREE_AXIS_COMPASS (0x0380)
-#define INV_ALL_SENSORS (0x7FFF)
-
-#ifdef INVENSENSE_COMPASS_CAL
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
- | INV_THREE_AXIS_COMPASS \
- | INV_THREE_AXIS_GYRO)
-#else
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
- | INV_THREE_AXIS_COMPASS \
- | INV_THREE_AXIS_GYRO)
-#endif
-
-// bit mask of current active features (mFeatureActiveMask)
-#define INV_COMPASS_CAL 0x01
-#define INV_COMPASS_FIT 0x02
-#define INV_DMP_QUATERNION 0x04
-#define INV_DMP_DISPL_ORIENTATION 0x08
-
-/* Uncomment to enable Low Power Quaternion */
-//#define ENABLE_LP_QUAT_FEAT
-
-/* Uncomment to enable DMP display orientation
- (within the HAL, see below for Java framework) */
-//#define ENABLE_DMP_DISPL_ORIENT_FEAT
-
-#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
-/* 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 isDmpScreenAutoRotationEnabled()
-{
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- return 1;
-#else
- return 0;
-#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,
- Accelerometer,
- MagneticField,
- Orientation,
- RotationVector,
- LinearAccel,
- Gravity,
- NUMSENSORS
- };
-
- MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
- virtual ~MPLSensor();
-
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int enable(int32_t handle, int enabled);
- int32_t getEnableMask() { return mEnabled; }
-
- virtual int readEvents(sensors_event_t *data, int count);
- virtual int getFd() const;
- virtual int getAccelFd() const;
- virtual int getCompassFd() const;
- virtual int getPollTime();
- virtual bool hasPendingEvents() const;
- virtual void sleepEvent();
- virtual void wakeEvent();
- int populateSensorList(struct sensor_t *list, int len);
- void cbProcData();
-
- //static pointer to the object that will handle callbacks
- static MPLSensor* gMPLSensor;
-
- //AKM HAL Integration
- //void set_compass(long ready, long x, long y, long z, long accuracy);
- int executeOnData(sensors_event_t* data, int count);
- int readAccelEvents(sensors_event_t* data, int count);
- int readCompassEvents(sensors_event_t* data, int count);
-
- int turnOffAccelFifo();
- int enableDmpOrientation(int);
- int dmpOrientHandler(int);
- int readDmpOrientEvents(sensors_event_t* data, int count);
- int getDmpOrientFd();
- int openDmpOrientFd();
- int closeDmpOrientFd();
-
- int getDmpRate(int64_t *);
- int checkDMPOrientation();
-
-protected:
- CompassSensor *mCompassSensor;
-
- int gyroHandler(sensors_event_t *data);
- int rawGyroHandler(sensors_event_t *data);
- int accelHandler(sensors_event_t *data);
- int compassHandler(sensors_event_t *data);
- int rvHandler(sensors_event_t *data);
- int laHandler(sensors_event_t *data);
- int gravHandler(sensors_event_t *data);
- int orienHandler(sensors_event_t *data);
- void calcOrientationSensor(float *Rx, float *Val);
- virtual int update_delay();
-
- void inv_set_device_properties();
- int inv_constructor_init();
- int inv_constructor_default_enable();
- int setGyroInitialState();
- int setAccelInitialState();
- int masterEnable(int en);
- int onPower(int en);
- int enableLPQuaternion(int);
- int enableQuaternionData(int);
- int onDMP(int);
- int enableGyro(int en);
- int enableAccel(int en);
- int enableCompass(int en);
- void computeLocalSensorMask(int enabled_sensors);
- int enableSensors(unsigned long sensors, int en, uint32_t changed);
- int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
- int inv_float_to_q16(float *fdata, long *ldata);
- int inv_long_to_q16(long *fdata, long *ldata);
- int inv_float_to_round(float *fdata, long *ldata);
- int inv_float_to_round2(float *fdata, short *sdata);
- int inv_read_temperature(long long *data);
- int inv_read_dmp_state(int fd);
- int inv_read_sensor_bias(int fd, long *data);
- void inv_get_sensors_orientation(void);
- int inv_init_sysfs_attributes(void);
-#ifdef COMPASS_YAS53x
- int resetCompass(void);
-#endif
- void setCompassDelay(int64_t ns);
- void enable_iio_sysfs(void);
- int enableTap(int);
- int enableFlick(int);
- int enablePedometer(int);
- int checkLPQuaternion();
-
- int mNewData; // flag indicating that the MPL calculated new output values
- int mDmpStarted;
- long mMasterSensorMask;
- long mLocalSensorMask;
- int mPollTime;
- 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;
- pthread_mutex_t mHALMutex;
-
- char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
-
- int iio_fd;
- int accel_fd;
- int mpufifo_fd;
- int gyro_temperature_fd;
-
- int dmp_orient_fd;
- int mDmpOrientationEnabled;
-
- uint32_t mEnabled;
- uint32_t mOldEnabledMask;
- sensors_event_t mPendingEvents[NUMSENSORS];
- int64_t mDelays[NUMSENSORS];
- hfunc_t mHandlers[NUMSENSORS];
- short mCachedGyroData[3];
- long mCachedAccelData[3];
- long mCachedCompassData[3];
- long mCachedQuaternionData[4];
- android::KeyedVector<int, int> mIrqFds;
-
- InputEventCircularReader mAccelInputReader;
- InputEventCircularReader mGyroInputReader;
-
- bool mFirstRead;
- short mTempScale;
- short mTempOffset;
- int64_t mTempCurrentTime;
- int mAccelScale;
-
- uint32_t mPendingMask;
- unsigned long mSensorMask;
-
- char chip_ID[MAX_CHIP_ID_LEN];
-
- signed char mGyroOrientation[9];
- signed char mAccelOrientation[9];
-
- int64_t mSensorTimestamp;
- int64_t mCompassTimestamp;
-
- struct sysfs_attrbs {
- char *chip_enable;
- char *power_state;
- char *dmp_firmware;
- char *firmware_loaded;
- char *dmp_on;
- char *dmp_int_on;
- char *dmp_event_int_on;
- char *dmp_output_rate;
- char *tap_on;
- char *key;
- char *self_test;
- char *temperature;
-
- char *gyro_enable;
- char *gyro_fifo_rate;
- char *gyro_orient;
- char *gyro_x_fifo_enable;
- char *gyro_y_fifo_enable;
- char *gyro_z_fifo_enable;
-
- char *accel_enable;
- char *accel_fifo_rate;
- char *accel_fsr;
- char *accel_bias;
- char *accel_orient;
- char *accel_x_fifo_enable;
- char *accel_y_fifo_enable;
- char *accel_z_fifo_enable;
-
- char *quaternion_on;
- char *in_quat_r_en;
- char *in_quat_x_en;
- char *in_quat_y_en;
- char *in_quat_z_en;
-
- char *in_timestamp_en;
- char *trigger_name;
- char *current_trigger;
- char *buffer_length;
-
- char *display_orientation_on;
- char *event_display_orientation;
- } mpu;
-
- char *sysfs_names_ptr;
- int mFeatureActiveMask;
-
-private:
- /* added for dynamic get sensor list */
- void fillAccel(const char* accel, struct sensor_t *list);
- void fillGyro(const char* gyro, struct sensor_t *list);
- void fillRV(struct sensor_t *list);
- void fillOrientation(struct sensor_t *list);
- void fillGravity(struct sensor_t *list);
- void fillLinearAccel(struct sensor_t *list);
- void storeCalibration();
- void loadDMP();
- bool isMpu3050();
- int isLowPowerQuatEnabled();
- int isDmpDisplayOrientationOn();
-
-
-};
-
-extern "C" {
- void setCallbackObject(MPLSensor*);
- MPLSensor *getCallbackObject();
-}
-
-#endif // ANDROID_MPL_SENSOR_H
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SENSOR_H
+#define ANDROID_MPL_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+// TODO: change to wanted vendor
+#ifdef INVENSENSE_COMPASS_CAL
+
+#ifdef COMPASS_AMI306
+#warning "unified HAL for AMI306"
+#include "CompassSensor.IIO.AMI.h"
+#else
+#warning "unified HAL for MPU9150"
+#include "CompassSensor.IIO.9150.h"
+#endif
+
+#else
+#warning "unified HAL for AKM"
+#include "CompassSensor.AKM.h"
+#endif
+
+/*****************************************************************************/
+/* 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)
+#define INV_THREE_AXIS_COMPASS (0x0380)
+#define INV_ALL_SENSORS (0x7FFF)
+
+#ifdef INVENSENSE_COMPASS_CAL
+#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
+ | INV_THREE_AXIS_COMPASS \
+ | INV_THREE_AXIS_GYRO)
+#else
+// TODO: ID_M = 2 even for 3rd-party solution
+#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
+ | INV_THREE_AXIS_COMPASS \
+ | INV_THREE_AXIS_GYRO)
+#endif
+
+// bit mask of current active features (mFeatureActiveMask)
+#define INV_COMPASS_CAL 0x01
+#define INV_COMPASS_FIT 0x02
+#define INV_DMP_QUATERNION 0x04
+
+/*****************************************************************************/
+/** 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:
+ MPLSensor(CompassSensor *);
+ virtual ~MPLSensor();
+
+ enum
+ {
+ Gyro = 0,
+ Accelerometer,
+ MagneticField,
+ Orientation,
+ RotationVector,
+ LinearAccel,
+ Gravity,
+ numSensors
+ };
+
+ virtual int setDelay(int32_t handle, int64_t ns);
+ virtual int enable(int32_t handle, int enabled);
+ int32_t getEnableMask() { return mEnabled; }
+
+ virtual int readEvents(sensors_event_t *data, int count);
+ virtual int getFd() const;
+ virtual int getAccelFd() const;
+ virtual int getCompassFd() const;
+ virtual int getPollTime();
+ virtual bool hasPendingEvents() const;
+ virtual void sleepEvent();
+ virtual void wakeEvent();
+ 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;
+
+ //AKM HAL Integration
+ //void set_compass(long ready, long x, long y, long z, long accuracy);
+ int executeOnData(sensors_event_t* data, int count);
+ int readAccelEvents(sensors_event_t* data, int count);
+ int readCompassEvents(sensors_event_t* data, int count);
+
+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);
+ int accelHandler(sensors_event_t *data);
+ int compassHandler(sensors_event_t *data);
+ int rvHandler(sensors_event_t *data);
+ int laHandler(sensors_event_t *data);
+ int gravHandler(sensors_event_t *data);
+ int orienHandler(sensors_event_t *data);
+ void calcOrientationSensor(float *Rx, float *Val);
+ virtual int update_delay();
+
+ void inv_set_device_properties();
+ int inv_constructor_init();
+ int inv_constructor_default_enable();
+ int setGyroInitialState();
+ int setAccelInitialState();
+ int masterEnable(int en);
+ int onPower(int en);
+ int enableLPQuaternion(int);
+ int enableQuaternionData(int);
+ int onDMP(int);
+ int enableGyro(int en);
+ int enableAccel(int en);
+ int enableCompass(int en);
+ void computeLocalSensorMask(int enabled_sensors);
+ int enableSensors(unsigned long sensors, int en, uint32_t changed);
+ int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
+ int update_delay_sysfs_sensor(int fd, uint64_t ns);
+ int inv_float_to_q16(float *fdata, long *ldata);
+ int inv_long_to_q16(long *fdata, long *ldata);
+ int inv_float_to_round(float *fdata, long *ldata);
+ int inv_float_to_round2(float *fdata, short *sdata);
+ int inv_read_temperature(long long *data);
+ int inv_read_dmp_state(int fd);
+ int inv_read_sensor_bias(int fd, long *data);
+ void inv_get_sensors_orientation(void);
+ int inv_init_sysfs_attributes(void);
+ void setCompassDelay(int64_t ns);
+ void enable_iio_sysfs(void);
+ int enableTap(int);
+ int enableFlick(int);
+ int enablePedometer(int);
+ int checkLPQuaternion();
+
+ int mNewData; // flag indicating that the MPL calculated new output values
+ int mDmpStarted;
+ long mMasterSensorMask;
+ long mLocalSensorMask;
+ int mPollTime;
+ 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.
+ struct pollfd mPollFds[5];
+ int mSampleCount;
+ pthread_mutex_t mMplMutex;
+ pthread_mutex_t mHALMutex;
+
+ char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
+
+ int iio_fd;
+ int accel_fd;
+ int mpufifo_fd;
+ int gyro_temperature_fd;
+
+ uint32_t mEnabled;
+ uint32_t mOldEnabledMask;
+ sensors_event_t mPendingEvents[numSensors];
+ uint64_t mDelays[numSensors];
+ hfunc_t mHandlers[numSensors];
+ short mCachedGyroData[3];
+ long mCachedAccelData[3];
+ long mCachedCompassData[3];
+ long mCachedQuaternionData[4];
+ android::KeyedVector<int, int> mIrqFds;
+
+ InputEventCircularReader mAccelInputReader;
+ InputEventCircularReader mGyroInputReader;
+
+ bool mFirstRead;
+ short mTempScale;
+ short mTempOffset;
+ int64_t mTempCurrentTime;
+ int mAccelScale;
+
+ uint32_t mPendingMask;
+ unsigned long mSensorMask;
+
+ char chip_ID[MAX_CHIP_ID_LEN];
+
+ signed char mGyroOrientation[9];
+ signed char mAccelOrientation[9];
+
+ int64_t mSensorTimestamp;
+ int64_t mCompassTimestamp;
+
+ struct sysfs_attrbs {
+ char *chip_enable;
+ char *power_state;
+ char *dmp_firmware;
+ char *firmware_loaded;
+ char *dmp_on;
+ char *dmp_int_on;
+ char *tap_on;
+ char *key;
+ char *self_test;
+ char *temperature;
+ char *dmp_output_rate;
+
+ char *gyro_enable;
+ char *gyro_fifo_rate;
+ char *gyro_orient;
+ char *gyro_x_fifo_enable;
+ char *gyro_y_fifo_enable;
+ char *gyro_z_fifo_enable;
+
+ char *accel_enable;
+ char *accel_fifo_rate;
+ char *accel_fsr;
+ char *accel_bias;
+ char *accel_orient;
+ char *accel_x_fifo_enable;
+ char *accel_y_fifo_enable;
+ char *accel_z_fifo_enable;
+
+ char *quaternion_on;
+ char *in_quat_r_en;
+ char *in_quat_x_en;
+ char *in_quat_y_en;
+ char *in_quat_z_en;
+
+ char *in_timestamp_en;
+ char *trigger_name;
+ char *current_trigger;
+ char *buffer_length;
+ } mpu;
+
+ char *sysfs_names_ptr;
+ int mFeatureActiveMask;
+
+private:
+ /* added for dynamic get sensor list */
+ void fillAccel(const char* accel, struct sensor_t *list);
+ void fillGyro(const char* gyro, struct sensor_t *list);
+ void fillRV(struct sensor_t *list);
+ void fillOrientation(struct sensor_t *list);
+ void fillGravity(struct sensor_t *list);
+ void fillLinearAccel(struct sensor_t *list);
+ void storeCalibration();
+ void loadDMP();
+};
+
+extern "C" {
+ void setCallbackObject(MPLSensor*);
+ MPLSensor *getCallbackObject();
+}
+
+#endif // ANDROID_MPL_SENSOR_H
diff --git a/libsensors_iio/MPLSupport.cpp b/libsensors_iio/MPLSupport.cpp
index c0b7d2a..a961d78 100644
--- a/libsensors_iio/MPLSupport.cpp
+++ b/libsensors_iio/MPLSupport.cpp
@@ -1,19 +1,3 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
#include <MPLSupport.h>
#include <string.h>
#include <stdio.h>
@@ -21,8 +5,6 @@
#include "SensorBase.h"
#include <fcntl.h>
-#include "ml_sysfs_helper.h"
-
int inv_read_data(char *fname, long *data)
{
VFUNC_LOG;
@@ -83,13 +65,20 @@
int err = 0;
if (fd >= 0) {
- char c = en ? '1' : '0';
- nb = write(fd, &c, 1);
+ char buf[2];
+ if (en) {
+ buf[0] = '1';
+ nb = write(fd, buf, 1);
+ } else {
+ buf[0] = '0';
+ nb = write(fd, buf, 1);
+ }
+ buf[1] = '\0';
if (nb <= 0) {
err = errno;
LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
- c, nb, strerror(err), err);
+ buf[0], nb, strerror(err), err);
}
close(fd);
} else {
@@ -122,28 +111,6 @@
return num_b;
}
-/* This one DOES NOT close FDs for you */
-int write_attribute_sensor_continuous(int fd, long data)
-{
- VFUNC_LOG;
-
- int num_b = 0;
-
- if (fd >= 0) {
- char buf[80];
- sprintf(buf, "%ld", data);
- num_b = write(fd, buf, strlen(buf) + 1);
- if (num_b <= 0) {
- int err = errno;
- LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
- } else {
- LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
- }
- }
-
- return num_b;
-}
-
int read_sysfs_int(char *filename, int *var)
{
int res=0;
@@ -152,10 +119,10 @@
sysfsfp = fopen(filename, "r");
if (sysfsfp!=NULL) {
fscanf(sysfsfp, "%d\n", var);
- fclose(sysfsfp);
+ fclose(sysfsfp);
} else {
- res = errno;
- LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+ LOGE("HAL:ERR open file to read");
+ res= -1;
}
return res;
}
@@ -168,54 +135,10 @@
sysfsfp = fopen(filename, "w");
if (sysfsfp!=NULL) {
fprintf(sysfsfp, "%d\n", var);
- fclose(sysfsfp);
+ fclose(sysfsfp);
} else {
- res = errno;
- LOGE("HAL:ERR open file %s to read with error %d", filename, res);
+ LOGE("HAL:ERR open file to write");
+ res= -1;
}
return res;
}
-
-int fill_dev_full_name_by_prefix(const char* dev_prefix,
- char *dev_full_name, int len)
-{
- char cand_name[20];
- int prefix_len = strlen(dev_prefix);
- strncpy(cand_name, dev_prefix, sizeof(cand_name) / sizeof(cand_name[0]));
-
- // try adding a number, 0-9
- for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
- snprintf(&cand_name[prefix_len],
- sizeof(cand_name) / sizeof(cand_name[0]),
- "%d", cand_postfix);
- int dev_num = find_type_by_name(cand_name, "iio:device");
- if (dev_num != -ENODEV) {
- strncpy(dev_full_name, cand_name, len);
- return 0;
- }
- }
- // try adding a small letter, a-z
- for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
- snprintf(&cand_name[prefix_len],
- sizeof(cand_name) / sizeof(cand_name[0]),
- "%c", cand_postfix);
- int dev_num = find_type_by_name(cand_name, "iio:device");
- if (dev_num != -ENODEV) {
- strncpy(dev_full_name, cand_name, len);
- return 0;
- }
- }
- // try adding a capital letter, A-Z
- for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
- snprintf(&cand_name[prefix_len],
- sizeof(cand_name) / sizeof(cand_name[0]),
- "%c", cand_postfix);
- int dev_num = find_type_by_name(cand_name, "iio:device");
- if (dev_num != -ENODEV) {
- strncpy(dev_full_name, cand_name, len);
- return 0;
- }
- }
- return 1;
-}
-
diff --git a/libsensors_iio/MPLSupport.h b/libsensors_iio/MPLSupport.h
index 42d7643..73604ba 100644
--- a/libsensors_iio/MPLSupport.h
+++ b/libsensors_iio/MPLSupport.h
@@ -19,14 +19,11 @@
#include <stdint.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 write_attribute_sensor_continuous(int fd, long data);
-int read_sysfs_int(char*, int*);
-int write_sysfs_int(char*, int);
-int fill_dev_full_name_by_prefix(const char* dev_prefix,
- char* dev_full_name, int len);
+ 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 0cf17af..fd0e2ca 100644
--- a/libsensors_iio/SensorBase.cpp
+++ b/libsensors_iio/SensorBase.cpp
@@ -1,142 +1,143 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <unistd.h>
-#include <dirent.h>
-#include <sys/select.h>
-#include <cutils/log.h>
-#include <linux/input.h>
-
-#include "SensorBase.h"
-
-/*****************************************************************************/
-
-SensorBase::SensorBase(const char* dev_name,
- const char* data_name) : dev_name(dev_name),
- data_name(data_name),
- dev_fd(-1),
- data_fd(-1)
-{
- if (data_name) {
- data_fd = openInput(data_name);
- }
-}
-
-SensorBase::~SensorBase()
-{
- if (data_fd >= 0) {
- close(data_fd);
- }
- if (dev_fd >= 0) {
- close(dev_fd);
- }
-}
-
-int SensorBase::open_device()
-{
- if (dev_fd<0 && dev_name) {
- dev_fd = open(dev_name, O_RDONLY);
- LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
- }
- return 0;
-}
-
-int SensorBase::close_device()
-{
- if (dev_fd >= 0) {
- close(dev_fd);
- dev_fd = -1;
- }
- return 0;
-}
-
-int SensorBase::getFd() const
-{
- if (!data_name) {
- return dev_fd;
- }
- return data_fd;
-}
-
-int SensorBase::setDelay(int32_t handle, int64_t ns)
-{
- return 0;
-}
-
-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;
- const char *dirname = "/dev/input";
- char devname[PATH_MAX];
- char *filename;
- DIR *dir;
- struct dirent *de;
- dir = opendir(dirname);
- if(dir == NULL)
- return -1;
- strcpy(devname, dirname);
- filename = devname + strlen(devname);
- *filename++ = '/';
- while((de = readdir(dir))) {
- if(de->d_name[0] == '.' &&
- (de->d_name[1] == '\0' ||
- (de->d_name[1] == '.' && de->d_name[2] == '\0')))
- continue;
- strcpy(filename, de->d_name);
- fd = open(devname, O_RDONLY);
- LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
- LOGI("path open %s", devname);
- if (fd >= 0) {
- char name[80];
- if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
- name[0] = '\0';
- }
- LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
- if (!strcmp(name, inputName)) {
- strcpy(input_name, filename);
- break;
- } else {
- close(fd);
- fd = -1;
- }
- }
- }
- closedir(dir);
- LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
- return fd;
-}
-
-int SensorBase::enable(int32_t handle, int enabled)
-{
- return 0;
-}
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+
+#include "SensorBase.h"
+#include "local_log_def.h"
+
+/*****************************************************************************/
+
+SensorBase::SensorBase(const char* dev_name,
+ const char* data_name) : dev_name(dev_name),
+ data_name(data_name),
+ dev_fd(-1),
+ data_fd(-1)
+{
+ if (data_name) {
+ data_fd = openInput(data_name);
+ }
+}
+
+SensorBase::~SensorBase()
+{
+ if (data_fd >= 0) {
+ close(data_fd);
+ }
+ if (dev_fd >= 0) {
+ close(dev_fd);
+ }
+}
+
+int SensorBase::open_device()
+{
+ if (dev_fd<0 && dev_name) {
+ dev_fd = open(dev_name, O_RDONLY);
+ LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
+ }
+ return 0;
+}
+
+int SensorBase::close_device()
+{
+ if (dev_fd >= 0) {
+ close(dev_fd);
+ dev_fd = -1;
+ }
+ return 0;
+}
+
+int SensorBase::getFd() const
+{
+ if (!data_name) {
+ return dev_fd;
+ }
+ return data_fd;
+}
+
+int SensorBase::setDelay(int32_t handle, int64_t ns)
+{
+ return 0;
+}
+
+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;
+ const char *dirname = "/dev/input";
+ char devname[PATH_MAX];
+ char *filename;
+ DIR *dir;
+ struct dirent *de;
+ dir = opendir(dirname);
+ if(dir == NULL)
+ return -1;
+ strcpy(devname, dirname);
+ filename = devname + strlen(devname);
+ *filename++ = '/';
+ while((de = readdir(dir))) {
+ if(de->d_name[0] == '.' &&
+ (de->d_name[1] == '\0' ||
+ (de->d_name[1] == '.' && de->d_name[2] == '\0')))
+ continue;
+ strcpy(filename, de->d_name);
+ fd = open(devname, O_RDONLY);
+ LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
+ LOGI("path open %s", devname);
+ if (fd >= 0) {
+ char name[80];
+ if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
+ name[0] = '\0';
+ }
+ LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
+ if (!strcmp(name, inputName)) {
+ strcpy(input_name, filename);
+ break;
+ } else {
+ close(fd);
+ fd = -1;
+ }
+ }
+ }
+ closedir(dir);
+ LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
+ return fd;
+}
+
+int SensorBase::enable(int32_t handle, int enabled)
+{
+ return 0;
+}
diff --git a/libsensors_iio/SensorBase.h b/libsensors_iio/SensorBase.h
index ddd1a05..d9abe92 100644
--- a/libsensors_iio/SensorBase.h
+++ b/libsensors_iio/SensorBase.h
@@ -22,42 +22,9 @@
#include <sys/cdefs.h>
#include <sys/types.h>
-#if defined ANDROID_JELLYBEAN
-#warning "build for Jellybean"
-#define LOGV_IF ALOGV_IF
-#define LOGE_IF ALOGE_IF
-#define LOGI ALOGI
-#define LOGE ALOGE
-#define LOGV ALOGV
-#define LOGW ALOGW
-#else
-#warning "build for ICS or earlier version"
-#endif
-
-/* Log enablers, each of these independent */
-
-#define PROCESS_VERBOSE (0) /* process log messages */
-#define EXTRA_VERBOSE (0) /* verbose log messages */
-#define SYSFS_VERBOSE (0) /* log sysfs interactions as cat/echo for repro
- purpose on a shell */
-#define FUNC_ENTRY (0) /* log entry in all one-time functions */
-
-/* Note that enabling this logs may affect performance */
-#define HANDLER_ENTRY (0) /* log entry in all handler functions */
-#define ENG_VERBOSE (0) /* log some a lot more info about the internals */
-#define INPUT_DATA (0) /* log the data input from the events */
-#define HANDLER_DATA (0) /* log the data fetched from the handlers */
-
-#define FUNC_LOG \
- LOGV("%s", __PRETTY_FUNCTION__)
-#define VFUNC_LOG \
- LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)
-#define VHANDLER_LOG \
- LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
#define MAX_SYSFS_NAME_LEN (100)
-#define IIO_BUFFER_LENGTH (480)
/*****************************************************************************/
diff --git a/libsensors_iio/libmllite.so b/libsensors_iio/libmllite.so
index c23e5b9..ed13b13 100644
--- a/libsensors_iio/libmllite.so
+++ b/libsensors_iio/libmllite.so
Binary files differ
diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so
index 23b4721..e2ab461 100644
--- a/libsensors_iio/libmplmpu.so
+++ b/libsensors_iio/libmplmpu.so
Binary files differ
diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h
new file mode 100644
index 0000000..b08fdf1
--- /dev/null
+++ b/libsensors_iio/local_log_def.h
@@ -0,0 +1,46 @@
+#ifndef LOCAL_LOG_DEF_H
+#define LOCAL_LOG_DEF_H
+
+// -- workaround for different LOG definition in JellyBean --
+#define LOGV ALOGV
+#define LOGV_IF ALOGV_IF
+#define LOGD ALOGD
+#define LOGD_IF ALOGD_IF
+#define LOGI ALOGI
+#define LOGI_IF ALOGI_IF
+#define LOGW ALOGW
+#define LOGW_IF ALOGW_IF
+#define LOGE ALOGE
+#define LOGE_IF ALOGE_IF
+#define IF_LOGV IF_ALOGV
+#define IF_LOGD IF_ALOGD
+#define IF_LOGI IF_ALOGI
+#define IF_LOGW IF_ALOGW
+#define IF_LOGE IF_ALOGE
+#define LOG_ASSERT ALOG_ASSERT
+#define LOG ALOG
+#define IF_LOG IF_ALOG
+// -- Workaround End --
+
+/* Log enablers, each of these independent */
+
+#define PROCESS_VERBOSE (0) /* process log messages */
+#define EXTRA_VERBOSE (0) /* verbose log messages */
+#define SYSFS_VERBOSE (0) /* log sysfs interactions as cat/echo for repro
+ purpose on a shell */
+#define FUNC_ENTRY (0) /* log entry in all one-time functions */
+
+/* Note that enabling this logs may affect performance */
+#define HANDLER_ENTRY (0) /* log entry in all handler functions */
+#define ENG_VERBOSE (0) /* log some a lot more info about the internals */
+#define INPUT_DATA (0) /* log the data input from the events */
+#define HANDLER_DATA (0) /* log the data fetched from the handlers */
+
+#define FUNC_LOG \
+ LOGV("%s", __PRETTY_FUNCTION__)
+#define VFUNC_LOG \
+ LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)
+#define VHANDLER_LOG \
+ LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
+
+#endif
diff --git a/libsensors_iio/sensor_params.h b/libsensors_iio/sensor_params.h
index c5d6307..88d5ba0 100644
--- a/libsensors_iio/sensor_params.h
+++ b/libsensors_iio/sensor_params.h
@@ -1,194 +1,162 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef INV_SENSOR_PARAMS_H
-#define INV_SENSOR_PARAMS_H
-
-/* Physical parameters of the sensors supported by Invensense MPL */
-#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV)
-#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA)
-#define SENSORS_GRAVITY_HANDLE (ID_GR)
-#define SENSORS_GYROSCOPE_HANDLE (ID_GY)
-#define SENSORS_RAW_GYROSCOPE_HANDLE (ID_RG)
-#define SENSORS_ACCELERATION_HANDLE (ID_A)
-#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M)
-#define SENSORS_ORIENTATION_HANDLE (ID_O)
-#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO)
-
-/******************************************/
-//MPU9250 INV_COMPASS
-#define COMPASS_MPU9250_RANGE (9830.f)
-#define COMPASS_MPU9250_RESOLUTION (0.15f)
-#define COMPASS_MPU9250_POWER (10.f)
-#define COMPASS_MPU9250_MINDELAY (10000)
-//MPU9150 INV_COMPASS
-#define COMPASS_MPU9150_RANGE (9830.f)
-#define COMPASS_MPU9150_RESOLUTION (0.285f)
-#define COMPASS_MPU9150_POWER (10.f)
-#define COMPASS_MPU9150_MINDELAY (10000)
-//COMPASS_ID_AK8975
-#define COMPASS_AKM8975_RANGE (9830.f)
-#define COMPASS_AKM8975_RESOLUTION (0.285f)
-#define COMPASS_AKM8975_POWER (10.f)
-#define COMPASS_AKM8975_MINDELAY (10000)
-//COMPASS_ID_AK8963C
-#define COMPASS_AKM8963_RANGE (9830.f)
-#define COMPASS_AKM8963_RESOLUTION (0.15f)
-#define COMPASS_AKM8963_POWER (10.f)
-#define COMPASS_AKM8963_MINDELAY (10000)
-//COMPASS_ID_AMI30X
-#define COMPASS_AMI30X_RANGE (5461.f)
-#define COMPASS_AMI30X_RESOLUTION (0.9f)
-#define COMPASS_AMI30X_POWER (0.15f)
-//COMPASS_ID_AMI306
-#define COMPASS_AMI306_RANGE (5461.f)
-#define COMPASS_AMI306_RESOLUTION (0.9f)
-#define COMPASS_AMI306_POWER (0.15f)
-#define COMPASS_AMI306_MINDELAY (10000)
-//COMPASS_ID_YAS529
-#define COMPASS_YAS529_RANGE (19660.f)
-#define COMPASS_YAS529_RESOLUTION (0.012f)
-#define COMPASS_YAS529_POWER (4.f)
-//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)
-#define COMPASS_HMC5883_POWER (0.24f)
-//COMPASS_ID_LSM303DLH
-#define COMPASS_LSM303DLH_RANGE (10240.f)
-#define COMPASS_LSM303DLH_RESOLUTION (1.f)
-#define COMPASS_LSM303DLH_POWER (1.f)
-//COMPASS_ID_LSM303DLM
-#define COMPASS_LSM303DLM_RANGE (10240.f)
-#define COMPASS_LSM303DLM_RESOLUTION (1.f)
-#define COMPASS_LSM303DLM_POWER (1.f)
-//COMPASS_ID_MMC314X
-#define COMPASS_MMC314X_RANGE (400.f)
-#define COMPASS_MMC314X_RESOLUTION (2.f)
-#define COMPASS_MMC314X_POWER (0.55f)
-//COMPASS_ID_HSCDTD002B
-#define COMPASS_HSCDTD002B_RANGE (9830.f)
-#define COMPASS_HSCDTD002B_RESOLUTION (1.f)
-#define COMPASS_HSCDTD002B_POWER (1.f)
-//COMPASS_ID_HSCDTD004A
-#define COMPASS_HSCDTD004A_RANGE (9830.f)
-#define COMPASS_HSCDTD004A_RESOLUTION (1.f)
-#define COMPASS_HSCDTD004A_POWER (1.f)
-/*******************************************/
-//ACCEL_ID_MPU6500
-#define ACCEL_MPU6500_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU6500_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU6500_POWER (0.f)
-#define ACCEL_MPU6500_MINDELAY (1000)
-//ACCEL_ID_MPU9250
-#define ACCEL_MPU9250_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU9250_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU9250_POWER (0.f)
-#define ACCEL_MPU9250_MINDELAY (1000)
-//ACCEL_ID_MPU9150
-#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU9150_POWER (0.f)
-#define ACCEL_MPU9150_MINDELAY (1000)
-//ACCEL_ID_LIS331
-#define ACCEL_LIS331_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LIS331_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LIS331_POWER (1.f)
-//ACCEL_ID_LSM303DLX
-#define ACCEL_LSM303DLX_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_POWER (1.f)
-//ACCEL_ID_LIS3DH
-#define ACCEL_LIS3DH_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LIS3DH_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LIS3DH_POWER (1.f)
-//ACCEL_ID_KXSD9
-#define ACCEL_KXSD9_RANGE (2.5006f * GRAVITY_EARTH)
-#define ACCEL_KXSD9_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_KXSD9_POWER (1.f)
-//ACCEL_ID_KXTF9
-#define ACCEL_KXTF9_RANGE (1.f * GRAVITY_EARTH)
-#define ACCEL_KXTF9_RESOLUTION (0.033f * GRAVITY_EARTH)
-#define ACCEL_KXTF9_POWER (0.35f)
-//ACCEL_ID_BMA150
-#define ACCEL_BMA150_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA150_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_BMA150_POWER (0.2f)
-//ACCEL_ID_BMA222
-#define ACCEL_BMA222_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA222_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_BMA222_POWER (0.1f)
-//ACCEL_ID_BMA250
-#define ACCEL_BMA250_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA250_RESOLUTION (0.00391f * GRAVITY_EARTH)
-#define ACCEL_BMA250_POWER (0.139f)
-#define ACCEL_BMA250_MINDELAY (1000)
-//ACCEL_ID_ADXL34X
-#define ACCEL_ADXL34X_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_ADXL34X_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_ADXL34X_POWER (1.f)
-//ACCEL_ID_MMA8450
-#define ACCEL_MMA8450_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MMA8450_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_MMA8450_POWER (1.0f)
-//ACCEL_ID_MMA845X
-#define ACCEL_MMA845X_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MMA845X_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_MMA845X_POWER (1.f)
-//ACCEL_ID_MPU6050
-#define ACCEL_MPU6050_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU6050_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU6050_POWER (0.f)
-#define ACCEL_MPU6050_MINDELAY (1000)
-/******************************************/
-//GYRO MPU3050
-#define RAD_P_DEG (3.14159f / 180.f)
-#define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU3050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU3050_POWER (6.1f)
-#define GYRO_MPU3050_MINDELAY (1000)
-//GYRO MPU6050
-#define GYRO_MPU6050_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU6050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU6050_POWER (5.5f)
-#define GYRO_MPU6050_MINDELAY (1000)
-//GYRO MPU9150
-#define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU9150_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU9150_POWER (5.5f)
-#define GYRO_MPU9150_MINDELAY (1000)
-//GYRO MPU9250
-#define GYRO_MPU9250_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU9250_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU9250_POWER (5.5f)
-#define GYRO_MPU9250_MINDELAY (1000)
-//GYRO MPU6500
-#define GYRO_MPU6500_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU6500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU6500_POWER (5.5f)
-#define GYRO_MPU6500_MINDELAY (1000)
-//GYRO ITG3500
-#define GYRO_ITG3500_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_ITG3500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_ITG3500_POWER (5.5f)
-#define GYRO_ITG3500_MINDELAY (1000)
-
-#endif /* INV_SENSOR_PARAMS_H */
-
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef INV_SENSOR_PARAMS_H
+#define INV_SENSOR_PARAMS_H
+
+/* Physical parameters of the sensors supported by Invensense MPL */
+#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV)
+#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA)
+#define SENSORS_GRAVITY_HANDLE (ID_GR)
+#define SENSORS_GYROSCOPE_HANDLE (ID_GY)
+#define SENSORS_ACCELERATION_HANDLE (ID_A)
+#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M)
+#define SENSORS_ORIENTATION_HANDLE (ID_O)
+
+/******************************************/
+//MPU9150 INV_COMPASS
+#define COMPASS_MPU9150_RANGE (9830.f)
+#define COMPASS_MPU9150_RESOLUTION (0.285f)
+#define COMPASS_MPU9150_POWER (10.f)
+#define COMPASS_MPU9150_MINDELAY (10000)
+//COMPASS_ID_AKM
+#define COMPASS_AKM8975_RANGE (9830.f)
+#define COMPASS_AKM8975_RESOLUTION (0.285f)
+#define COMPASS_AKM8975_POWER (10.f)
+#define COMPASS_AKM8975_MINDELAY (10000)
+//COMPASS_ID_AMI30X
+#define COMPASS_AMI30X_RANGE (5461.f)
+#define COMPASS_AMI30X_RESOLUTION (0.9f)
+#define COMPASS_AMI30X_POWER (0.15f)
+//COMPASS_ID_AMI306
+#define COMPASS_AMI306_RANGE (5461.f)
+#define COMPASS_AMI306_RESOLUTION (0.9f)
+#define COMPASS_AMI306_POWER (0.15f)
+#define COMPASS_AMI306_MINDELAY (10000)
+//COMPASS_ID_YAS529
+#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_HMC5883
+#define COMPASS_HMC5883_RANGE (10673.f)
+#define COMPASS_HMC5883_RESOLUTION (10.f)
+#define COMPASS_HMC5883_POWER (0.24f)
+//COMPASS_ID_LSM303DLH
+#define COMPASS_LSM303DLH_RANGE (10240.f)
+#define COMPASS_LSM303DLH_RESOLUTION (1.f)
+#define COMPASS_LSM303DLH_POWER (1.f)
+//COMPASS_ID_LSM303DLM
+#define COMPASS_LSM303DLM_RANGE (10240.f)
+#define COMPASS_LSM303DLM_RESOLUTION (1.f)
+#define COMPASS_LSM303DLM_POWER (1.f)
+//COMPASS_ID_MMC314X
+#define COMPASS_MMC314X_RANGE (400.f)
+#define COMPASS_MMC314X_RESOLUTION (2.f)
+#define COMPASS_MMC314X_POWER (0.55f)
+//COMPASS_ID_HSCDTD002B
+#define COMPASS_HSCDTD002B_RANGE (9830.f)
+#define COMPASS_HSCDTD002B_RESOLUTION (1.f)
+#define COMPASS_HSCDTD002B_POWER (1.f)
+//COMPASS_ID_HSCDTD004A
+#define COMPASS_HSCDTD004A_RANGE (9830.f)
+#define COMPASS_HSCDTD004A_RESOLUTION (1.f)
+#define COMPASS_HSCDTD004A_POWER (1.f)
+/*******************************************/
+//ACCEL_ID_MPU9150
+#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_POWER (0.f)
+#define ACCEL_MPU9150_MINDELAY (1000)
+//ACCEL_ID_LIS331
+#define ACCEL_LIS331_RANGE (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS331_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS331_POWER (1.f)
+//ACCEL_ID_LSM303DLX
+#define ACCEL_LSM303DLX_RANGE (2.48f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_POWER (1.f)
+//ACCEL_ID_LIS3DH
+#define ACCEL_LIS3DH_RANGE (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_POWER (1.f)
+//ACCEL_ID_KXSD9
+#define ACCEL_KXSD9_RANGE (2.5006f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_POWER (1.f)
+//ACCEL_ID_KXTF9
+#define ACCEL_KXTF9_RANGE (1.f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_RESOLUTION (0.033f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_POWER (0.35f)
+//ACCEL_ID_BMA150
+#define ACCEL_BMA150_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA150_RESOLUTION (0.004f * GRAVITY_EARTH)
+#define ACCEL_BMA150_POWER (0.2f)
+//ACCEL_ID_BMA222
+#define ACCEL_BMA222_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA222_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_BMA222_POWER (0.1f)
+//ACCEL_ID_BMA250
+#define ACCEL_BMA250_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA250_RESOLUTION (0.00391f * GRAVITY_EARTH)
+#define ACCEL_BMA250_POWER (0.139f)
+#define ACCEL_BMA250_MINDELAY (1000)
+//ACCEL_ID_ADXL34X
+#define ACCEL_ADXL34X_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_POWER (1.f)
+//ACCEL_ID_MMA8450
+#define ACCEL_MMA8450_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_POWER (1.0f)
+//ACCEL_ID_MMA845X
+#define ACCEL_MMA845X_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_POWER (1.f)
+//ACCEL_ID_MPU6050
+#define ACCEL_MPU6050_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_RESOLUTION (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_POWER (0.f)
+#define ACCEL_MPU6050_MINDELAY (1000)
+/******************************************/
+//GYRO MPU3050
+#define RAD_P_DEG (3.14159f / 180.f)
+#define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_MPU3050_RESOLUTION (32.8f * RAD_P_DEG)
+#define GYRO_MPU3050_POWER (6.1f)
+#define GYRO_MPU3050_MINDELAY (1000)
+//GYRO MPU6050
+#define GYRO_MPU6050_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_MPU6050_RESOLUTION (16.4f * RAD_P_DEG)
+#define GYRO_MPU6050_POWER (5.5f)
+#define GYRO_MPU6050_MINDELAY (1000)
+//GYRO MPU9150
+#define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_MPU9150_RESOLUTION (16.4f * RAD_P_DEG)
+#define GYRO_MPU9150_POWER (5.5f)
+#define GYRO_MPU9150_MINDELAY (1000)
+//GYRO ITG3500
+#define GYRO_ITG3500_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_ITG3500_RESOLUTION (16.4f * RAD_P_DEG)
+#define GYRO_ITG3500_POWER (5.5f)
+#define GYRO_ITG3500_MINDELAY (1000)
+
+#endif /* INV_SENSOR_PARAMS_H */
+
diff --git a/libsensors_iio/sensors.h b/libsensors_iio/sensors.h
index d07e956..0556c10 100644
--- a/libsensors_iio/sensors.h
+++ b/libsensors_iio/sensors.h
@@ -1,97 +1,104 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_SENSORS_H
-#define ANDROID_SENSORS_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <hardware/hardware.h>
-#include <hardware/sensors.h>
-
-__BEGIN_DECLS
-
-/*****************************************************************************/
-
-#ifndef ARRAY_SIZE
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
-#endif
-
-enum {
- ID_GY = 0,
- ID_RG,
- ID_A,
- ID_M,
- ID_O,
- ID_RV,
- ID_LA,
- ID_GR,
- ID_SO
-};
-
-/*****************************************************************************/
-
-/*
- * The SENSORS Module
- */
-
-/* ITG3500 */
-#define EVENT_TYPE_GYRO_X REL_X
-#define EVENT_TYPE_GYRO_Y REL_Y
-#define EVENT_TYPE_GYRO_Z REL_Z
-/* MPU6050 MPU9150 */
-#define EVENT_TYPE_IACCEL_X REL_RX
-#define EVENT_TYPE_IACCEL_Y REL_RY
-#define EVENT_TYPE_IACCEL_Z REL_RZ
-/* MPU6050 MPU9150 */
-#define EVENT_TYPE_ICOMPASS_X REL_X
-#define EVENT_TYPE_ICOMPASS_Y REL_Y
-#define EVENT_TYPE_ICOMPASS_Z REL_Z
-/* MPUxxxx */
-#define EVENT_TYPE_TIMESTAMP_HI REL_MISC
-#define EVENT_TYPE_TIMESTAMP_LO REL_WHEEL
-
-/* Accel BMA250 */
-#define EVENT_TYPE_ACCEL_X ABS_X
-#define EVENT_TYPE_ACCEL_Y ABS_Y
-#define EVENT_TYPE_ACCEL_Z ABS_Z
-#define LSG (1000.0f)
-
-// conversion of acceleration data to SI units (m/s^2)
-#define RANGE_A (4*GRAVITY_EARTH)
-#define RESOLUTION_A (GRAVITY_EARTH / LSG)
-#define CONVERT_A (GRAVITY_EARTH / LSG)
-#define CONVERT_A_X (CONVERT_A)
-#define CONVERT_A_Y (CONVERT_A)
-#define CONVERT_A_Z (CONVERT_A)
-
-/* Compass AKM8975 */
-#define EVENT_TYPE_MAGV_X ABS_RX
-#define EVENT_TYPE_MAGV_Y ABS_RY
-#define EVENT_TYPE_MAGV_Z ABS_RZ
-#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER
-
-// conversion of magnetic data to uT units
-#define CONVERT_M (0.06f)
-
-__END_DECLS
-
-#endif // ANDROID_SENSORS_H
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_SENSORS_H
+#define ANDROID_SENSORS_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <hardware/hardware.h>
+#include <hardware/sensors.h>
+
+__BEGIN_DECLS
+
+/*****************************************************************************/
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+#endif
+
+#define ID_MPL_BASE (0)
+#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)
+
+/*#define ID_MPL_BASE (0)
+#define ID_RV (ID_MPL_BASE)
+#define ID_LA (ID_RV + 1)
+#define ID_GR (ID_LA + 1)
+#define ID_GY (ID_GR + 1)
+#define ID_A (ID_GY + 1)
+#define ID_M (ID_A + 1)
+#define ID_O (ID_M + 1)
+*/
+
+/*****************************************************************************/
+
+/*
+ * The SENSORS Module
+ */
+
+/* ITG3500 */
+#define EVENT_TYPE_GYRO_X REL_X
+#define EVENT_TYPE_GYRO_Y REL_Y
+#define EVENT_TYPE_GYRO_Z REL_Z
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_IACCEL_X REL_RX
+#define EVENT_TYPE_IACCEL_Y REL_RY
+#define EVENT_TYPE_IACCEL_Z REL_RZ
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_ICOMPASS_X REL_X
+#define EVENT_TYPE_ICOMPASS_Y REL_Y
+#define EVENT_TYPE_ICOMPASS_Z REL_Z
+/* MPUxxxx */
+#define EVENT_TYPE_TIMESTAMP_HI REL_MISC
+#define EVENT_TYPE_TIMESTAMP_LO REL_WHEEL
+
+/* Accel BMA250 */
+#define EVENT_TYPE_ACCEL_X ABS_X
+#define EVENT_TYPE_ACCEL_Y ABS_Y
+#define EVENT_TYPE_ACCEL_Z ABS_Z
+#define LSG (1000.0f)
+
+// conversion of acceleration data to SI units (m/s^2)
+#define RANGE_A (4*GRAVITY_EARTH)
+#define RESOLUTION_A (GRAVITY_EARTH / LSG)
+#define CONVERT_A (GRAVITY_EARTH / LSG)
+#define CONVERT_A_X (CONVERT_A)
+#define CONVERT_A_Y (CONVERT_A)
+#define CONVERT_A_Z (CONVERT_A)
+
+/* Compass AKM8975 */
+#define EVENT_TYPE_MAGV_X ABS_RX
+#define EVENT_TYPE_MAGV_Y ABS_RY
+#define EVENT_TYPE_MAGV_Z ABS_RZ
+#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER
+
+// conversion of magnetic data to uT units
+#define CONVERT_M (0.06f)
+
+__END_DECLS
+
+#endif // ANDROID_SENSORS_H
diff --git a/libsensors_iio/sensors_mpl.cpp b/libsensors_iio/sensors_mpl.cpp
index 50bf315..990c5f5 100644
--- a/libsensors_iio/sensors_mpl.cpp
+++ b/libsensors_iio/sensors_mpl.cpp
@@ -1,260 +1,236 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
-
-#include <hardware/sensors.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <dirent.h>
-#include <math.h>
-#include <poll.h>
-#include <pthread.h>
-#include <stdlib.h>
-
-#include <linux/input.h>
-
-#include <utils/Atomic.h>
-#include <utils/Log.h>
-
-#include "sensors.h"
-#include "MPLSensor.h"
-
-/*****************************************************************************/
-/* The SENSORS Module */
-
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
-#define LOCAL_SENSORS (MPLSensor::NUMSENSORS + 1)
-#else
-#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 = (sizeof(sSensorList) / sizeof(sensor_t));
-
-static int open_sensors(const struct hw_module_t* module, const char* id,
- struct hw_device_t** device);
-
-static int sensors__get_sensors_list(struct sensors_module_t* module,
- struct sensor_t const** list)
-{
- *list = sSensorList;
- return sensors;
-}
-
-static struct hw_module_methods_t sensors_module_methods = {
- open: open_sensors
-};
-
-struct sensors_module_t HAL_MODULE_INFO_SYM = {
- common: {
- tag: HARDWARE_MODULE_TAG,
- version_major: 1,
- version_minor: 0,
- id: SENSORS_HARDWARE_MODULE_ID,
- name: "Invensense module",
- author: "Invensense Inc.",
- methods: &sensors_module_methods,
- },
- get_sensors_list: sensors__get_sensors_list,
-};
-
-struct sensors_poll_context_t {
- struct sensors_poll_device_t device; // must be first
-
- sensors_poll_context_t();
- ~sensors_poll_context_t();
- int activate(int handle, int enabled);
- int setDelay(int handle, int64_t ns);
- int pollEvents(sensors_event_t* data, int count);
-
-private:
- enum {
- mpl = 0,
- compass,
- dmpOrient,
- numSensorDrivers, // wake pipe goes here
- numFds,
- };
-
- struct pollfd mPollFds[numSensorDrivers];
- SensorBase *mSensor;
- CompassSensor *mCompassSensor;
-};
-
-/******************************************************************************/
-
-sensors_poll_context_t::sensors_poll_context_t() {
- VFUNC_LOG;
-
- 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);
-
- // populate the sensor list
- sensors =
- mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
-
- mSensor = mplSensor;
- mPollFds[mpl].fd = mSensor->getFd();
- mPollFds[mpl].events = POLLIN;
- mPollFds[mpl].revents = 0;
-
- mPollFds[compass].fd = mCompassSensor->getFd();
- mPollFds[compass].events = POLLIN;
- mPollFds[compass].revents = 0;
-
- mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
- mPollFds[dmpOrient].events = POLLPRI;
- mPollFds[dmpOrient].revents = 0;
-}
-
-sensors_poll_context_t::~sensors_poll_context_t() {
- FUNC_LOG;
- delete mSensor;
- delete mCompassSensor;
-}
-
-int sensors_poll_context_t::activate(int handle, int enabled) {
- FUNC_LOG;
- return mSensor->enable(handle, enabled);
-}
-
-int sensors_poll_context_t::setDelay(int handle, int64_t ns)
-{
- FUNC_LOG;
- return mSensor->setDelay(handle, ns);
-}
-
-int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
-{
- VHANDLER_LOG;
-
- int nbEvents = 0;
- int nb, polltime = -1;
-
- // look for new events
- nb = poll(mPollFds, numSensorDrivers, polltime);
-
- if (nb > 0) {
- for (int i = 0; count && i < numSensorDrivers; i++) {
- if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
- nb = 0;
- if (i == mpl) {
- nb = mSensor->readEvents(NULL, 0);
- mPollFds[i].revents = 0;
- }
- else if (i == compass) {
- nb = ((MPLSensor*) mSensor)->readCompassEvents(NULL, 0);
- mPollFds[i].revents = 0;
- }
- }
- }
- nb = ((MPLSensor*) mSensor)->executeOnData(data, count);
- if (nb > 0) {
- count -= nb;
- nbEvents += nb;
- data += nb;
- }
-
- if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {
- nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);
- mPollFds[dmpOrient].revents= 0;
- if (isDmpScreenAutoRotationEnabled() && nb > 0) {
- count -= nb;
- nbEvents += nb;
- data += nb;
- }
- }
- }
-
- return nbEvents;
-}
-
-/******************************************************************************/
-
-static int poll__close(struct hw_device_t *dev)
-{
- FUNC_LOG;
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- if (ctx) {
- delete ctx;
- }
- return 0;
-}
-
-static int poll__activate(struct sensors_poll_device_t *dev,
- int handle, int enabled)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- return ctx->activate(handle, enabled);
-}
-
-static int poll__setDelay(struct sensors_poll_device_t *dev,
- int handle, int64_t ns)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- int s= ctx->setDelay(handle, ns);
- return s;
-}
-
-static int poll__poll(struct sensors_poll_device_t *dev,
- sensors_event_t* data, int count)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- return ctx->pollEvents(data, count);
-}
-
-/******************************************************************************/
-
-/** Open a new instance of a sensor device using name */
-static int open_sensors(const struct hw_module_t* module, const char* id,
- struct hw_device_t** device)
-{
- FUNC_LOG;
- int status = -EINVAL;
- sensors_poll_context_t *dev = new sensors_poll_context_t();
-
- memset(&dev->device, 0, sizeof(sensors_poll_device_t));
-
- dev->device.common.tag = HARDWARE_DEVICE_TAG;
- dev->device.common.version = 0;
- dev->device.common.module = const_cast<hw_module_t*>(module);
- dev->device.common.close = poll__close;
- dev->device.activate = poll__activate;
- dev->device.setDelay = poll__setDelay;
- dev->device.poll = poll__poll;
-
- *device = &dev->device.common;
- status = 0;
-
- return status;
-}
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
+
+#include <hardware/sensors.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <dirent.h>
+#include <math.h>
+#include <poll.h>
+#include <pthread.h>
+#include <stdlib.h>
+
+#include <linux/input.h>
+
+#include <utils/Atomic.h>
+#include <utils/Log.h>
+
+#include "sensors.h"
+#include "MPLSensor.h"
+#include "local_log_def.h"
+
+/*****************************************************************************/
+/* The SENSORS Module */
+#define LOCAL_SENSORS (7)
+
+static struct sensor_t sSensorList[7];
+static int numSensors = LOCAL_SENSORS;
+
+static int open_sensors(const struct hw_module_t* module, const char* id,
+ struct hw_device_t** device);
+
+static int sensors__get_sensors_list(struct sensors_module_t* module,
+ struct sensor_t const** list)
+{
+ *list = sSensorList;
+ return numSensors;
+}
+
+static struct hw_module_methods_t sensors_module_methods = {
+ open: open_sensors
+};
+
+struct sensors_module_t HAL_MODULE_INFO_SYM = {
+ common: {
+ tag: HARDWARE_MODULE_TAG,
+ version_major: 1,
+ version_minor: 0,
+ id: SENSORS_HARDWARE_MODULE_ID,
+ name: "Invensense module",
+ author: "Invensense Inc.",
+ methods: &sensors_module_methods,
+ },
+ get_sensors_list: sensors__get_sensors_list,
+};
+
+struct sensors_poll_context_t {
+ struct sensors_poll_device_t device; // must be first
+
+ sensors_poll_context_t();
+ ~sensors_poll_context_t();
+ int activate(int handle, int enabled);
+ int setDelay(int handle, int64_t ns);
+ int pollEvents(sensors_event_t* data, int count);
+
+private:
+ enum {
+ mpl = 0,
+ compass,
+ numSensorDrivers, // wake pipe goes here
+ numFds,
+ };
+
+ struct pollfd mPollFds[numSensorDrivers];
+ SensorBase *mSensor;
+ CompassSensor *mCompassSensor;
+};
+
+/******************************************************************************/
+
+sensors_poll_context_t::sensors_poll_context_t() {
+ VFUNC_LOG;
+
+ CompassSensor *mCompassSensor = new CompassSensor();
+ MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
+
+ // setup the callback object for handing mpl callbacks
+ setCallbackObject(mplSensor);
+
+ // populate the sensor list
+ numSensors =
+ mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
+
+ mSensor = mplSensor;
+ mPollFds[mpl].fd = mSensor->getFd();
+ mPollFds[mpl].events = POLLIN;
+ mPollFds[mpl].revents = 0;
+
+ mPollFds[compass].fd = mCompassSensor->getFd();
+ mPollFds[compass].events = POLLIN;
+ mPollFds[compass].revents = 0;
+}
+
+sensors_poll_context_t::~sensors_poll_context_t() {
+ FUNC_LOG;
+ delete mSensor;
+ delete mCompassSensor;
+}
+
+int sensors_poll_context_t::activate(int handle, int enabled) {
+ FUNC_LOG;
+ return mSensor->enable(handle, enabled);
+}
+
+int sensors_poll_context_t::setDelay(int handle, int64_t ns)
+{
+ FUNC_LOG;
+ return mSensor->setDelay(handle, ns);
+}
+
+int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
+{
+ VHANDLER_LOG;
+
+ int nbEvents = 0;
+ int nb, polltime = -1;
+
+ // look for new events
+ nb = poll(mPollFds, numSensorDrivers, polltime);
+
+ if (nb > 0) {
+ for (int i = 0; count && i < numSensorDrivers; i++) {
+ if (mPollFds[i].revents & POLLIN) {
+ nb = 0;
+ if (i == mpl) {
+ nb = mSensor->readEvents(data, count);
+ }
+ else if (i == compass) {
+ nb = ((MPLSensor*) mSensor)->readCompassEvents(data, count);
+ }
+/*
+ if (nb > 0) {
+ count -= nb;
+ nbEvents += nb;
+ data += nb;
+ mPollFds[i].revents = 0;
+ }
+ */
+ }
+ }
+ nb = ((MPLSensor*) mSensor)->executeOnData(data, count);
+ if (nb > 0) {
+ count -= nb;
+ nbEvents += nb;
+ data += nb;
+ mPollFds[mpl].revents = 0;
+ mPollFds[compass].revents = 0;
+ }
+ }
+
+ return nbEvents;
+}
+
+/******************************************************************************/
+
+static int poll__close(struct hw_device_t *dev)
+{
+ FUNC_LOG;
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ if (ctx) {
+ delete ctx;
+ }
+ return 0;
+}
+
+static int poll__activate(struct sensors_poll_device_t *dev,
+ int handle, int enabled)
+{
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ return ctx->activate(handle, enabled);
+}
+
+static int poll__setDelay(struct sensors_poll_device_t *dev,
+ int handle, int64_t ns)
+{
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ int s= ctx->setDelay(handle, ns);
+ return s;
+}
+
+static int poll__poll(struct sensors_poll_device_t *dev,
+ sensors_event_t* data, int count)
+{
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ return ctx->pollEvents(data, count);
+}
+
+/******************************************************************************/
+
+/** Open a new instance of a sensor device using name */
+static int open_sensors(const struct hw_module_t* module, const char* id,
+ struct hw_device_t** device)
+{
+ FUNC_LOG;
+ int status = -EINVAL;
+ sensors_poll_context_t *dev = new sensors_poll_context_t();
+
+ memset(&dev->device, 0, sizeof(sensors_poll_device_t));
+
+ dev->device.common.tag = HARDWARE_DEVICE_TAG;
+ dev->device.common.version = 0;
+ dev->device.common.module = const_cast<hw_module_t*>(module);
+ dev->device.common.close = poll__close;
+ dev->device.activate = poll__activate;
+ dev->device.setDelay = poll__setDelay;
+ dev->device.poll = poll__poll;
+
+ *device = &dev->device.common;
+ status = 0;
+
+ return status;
+}
diff --git a/libsensors_iio/software/build/android/common.mk b/libsensors_iio/software/build/android/common.mk
index 84e7e9b..b84a99c 100644
--- a/libsensors_iio/software/build/android/common.mk
+++ b/libsensors_iio/software/build/android/common.mk
@@ -4,9 +4,6 @@
####################################################################################################
## defines
-# Build for Jellybean
-BUILD_ANDROID_JELLYBEAN = 1
-
## libraries ##
LIB_PREFIX = lib
@@ -19,39 +16,23 @@
MLLITE_LIB_NAME ?= mllite
MPL_LIB_NAME ?= mplmpu
+HALWRAPPER_LIB_NAME ?= androidhal
## applications ##
SHARED_APP_SUFFIX = -shared
STATIC_APP_SUFFIX = -static
####################################################################################################
-## compile, includes, and linker
+## includes and linker
-ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
-ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
-endif
-
-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)/system/lib
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 # ICS
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
+ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include
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 47dedfe..bc8548c 100644
--- a/libsensors_iio/software/build/android/shared.mk
+++ b/libsensors_iio/software/build/android/shared.mk
@@ -22,9 +22,9 @@
ifeq ($(BUILD_MPL),1)
LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET)
endif
-APP_FOLDERS = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
+APP_FOLDERS = $(INV_ROOT)/simple_apps/console/linux/build/$(TARGET)
+APP_FOLDERS += $(INV_ROOT)/simple_apps/input_sub/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/HAL/include/inv_sysfs_utils.h b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h
new file mode 100644
index 0000000..fce28ae
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h
@@ -0,0 +1,83 @@
+/**
+ * @brief Provides helpful file IO wrappers for use with sysfs.
+ * @details Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#ifndef _INV_SYSFS_UTILS_H_
+#define _INV_SYSFS_UTILS_H_
+
+/**
+ * struct inv_sysfs_names_s - Files needed by user applications.
+ * @buffer: Ring buffer attached to FIFO.
+ * @enable: Turns on HW-to-ring buffer flow.
+ * @raw_data: Raw data from registers.
+ * @temperature: Temperature data from register.
+ * @fifo_rate: FIFO rate/ODR.
+ * @power_state: Power state (this is a five-star comment).
+ * @fsr: Full-scale range.
+ * @lpf: Digital low pass filter.
+ * @scale: LSBs / dps (or LSBs / Gs).
+ * @temp_scale: LSBs / degrees C.
+ * @temp_offset: Offset in LSBs.
+ */
+struct inv_sysfs_names_s {
+
+ //Sysfs for ITG3500 & MPU6050
+ const char *buffer;
+ const char *enable;
+ const char *raw_data; //Raw Gyro data
+ const char *temperature;
+ const char *fifo_rate;
+ const char *power_state;
+ const char *fsr;
+ const char *lpf;
+ const char *scale; //Gyro scale
+ const char *temp_scale;
+ const char *temp_offset;
+ const char *self_test;
+ //Starting Sysfs available for MPU6050 only
+ const char *accel_en;
+ const char *accel_fifo_en;
+ const char *accel_fs;
+ const char *clock_source;
+ const char *early_suspend_en;
+ const char *firmware_loaded;
+ const char *gyro_en;
+ const char *gyro_fifo_en;
+ const char *key;
+ const char *raw_accel;
+ const char *reg_dump;
+ const char *tap_on;
+ const char *dmp_firmware;
+};
+
+/* File IO. Typically won't be called directly by user application, but they'll
+ * be here for your enjoyment.
+ */
+int inv_sysfs_write(char *filename, long data);
+int inv_sysfs_read(char *filename, long num_bytes, char *data);
+
+/* Helper APIs to extract specific data. */
+int inv_read_buffer(int fd, long *data, long long *timestamp);
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data,
+ long long *timestamp);
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+ long long *timestamp);
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data);
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data);
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data);
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data);
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data);
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data);
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data);
+
+/* Scaled data. */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+ long long *timestamp);
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+ long long *timestamp);
+
+#endif /* #ifndef _INV_SYSFS_UTILS_H_ */
+
+
diff --git a/libsensors_iio/software/core/HAL/include/mlos.h b/libsensors_iio/software/core/HAL/include/mlos.h
new file mode 100644
index 0000000..ce06b07
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/include/mlos.h
@@ -0,0 +1,104 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef unsigned int HANDLE;
+#endif
+
+ /* ------------ */
+ /* - Defines. - */
+ /* ------------ */
+
+ /* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003)
+
+ /* ---------- */
+ /* - Enums. - */
+ /* ---------- */
+
+ /* --------------- */
+ /* - Structures. - */
+ /* --------------- */
+
+ /* --------------------- */
+ /* - Function p-types. - */
+ /* --------------------- */
+
+#ifndef __KERNEL__
+#include <string.h>
+ void *inv_malloc(unsigned int numBytes);
+ inv_error_t inv_free(void *ptr);
+ inv_error_t inv_create_mutex(HANDLE *mutex);
+ inv_error_t inv_lock_mutex(HANDLE mutex);
+ inv_error_t inv_unlock_mutex(HANDLE mutex);
+ FILE *inv_fopen(char *filename);
+ void inv_fclose(FILE *fp);
+
+ // Binary Semaphore
+ inv_error_t inv_create_binary_semaphore(HANDLE *semaphore);
+ inv_error_t inv_destroy_binary_semaphore(HANDLE handle);
+ inv_error_t inv_increment_binary_semaphore(HANDLE handle);
+ inv_error_t inv_decrement_binary_semaphore(HANDLE handle, long timeout_ms);
+
+ inv_error_t inv_destroy_mutex(HANDLE handle);
+
+ void inv_sleep(int mSecs);
+ inv_time_t inv_get_tick_count(void);
+
+ /* Kernel implmentations */
+#define GFP_KERNEL (0x70)
+ static inline void *kmalloc(size_t size,
+ unsigned int gfp_flags)
+ {
+ (void)gfp_flags;
+ return inv_malloc((unsigned int)size);
+ }
+ static inline void *kzalloc(size_t size, unsigned int gfp_flags)
+ {
+ void *tmp = inv_malloc((unsigned int)size);
+ (void)gfp_flags;
+ if (tmp)
+ memset(tmp, 0, size);
+ return tmp;
+ }
+ static inline void kfree(void *ptr)
+ {
+ inv_free(ptr);
+ }
+ static inline void msleep(long msecs)
+ {
+ inv_sleep(msecs);
+ }
+ static inline void udelay(unsigned long usecs)
+ {
+ inv_sleep((usecs + 999) / 1000);
+ }
+#else
+#include <linux/delay.h>
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* _MLOS_H */
diff --git a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c
new file mode 100644
index 0000000..c45badd
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c
@@ -0,0 +1,317 @@
+/**
+ * @brief Provides helpful file IO wrappers for use with sysfs.
+ * @details Based on Jonathan Cameron's @e iio_utils.h.
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <dirent.h>
+#include <errno.h>
+#include <unistd.h>
+#include "inv_sysfs_utils.h"
+
+/* General TODO list:
+ * Select more reasonable string lengths or use fseek and malloc.
+ */
+
+/**
+ * inv_sysfs_write() - Write an integer to a file.
+ * @filename: Path to file.
+ * @data: Value to write to file.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_write(char *filename, long data)
+{
+ FILE *fp;
+ int count;
+
+ if (!filename)
+ return -1;
+ fp = fopen(filename, "w");
+ if (!fp)
+ return -errno;
+ count = fprintf(fp, "%ld", data);
+ fclose(fp);
+ return count;
+}
+
+/**
+ * inv_sysfs_read() - Read a string from a file.
+ * @filename: Path to file.
+ * @num_bytes: Number of bytes to read.
+ * @data: Data from file.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+ FILE *fp;
+ int count;
+
+ if (!filename)
+ return -1;
+ fp = fopen(filename, "r");
+ if (!fp)
+ return -errno;
+ count = fread(data, 1, num_bytes, fp);
+ fclose(fp);
+ return count;
+}
+
+/**
+ * inv_read_buffer() - Read data from ring buffer.
+ * @fd: File descriptor for buffer file.
+ * @data: Data in hardware units.
+ * @timestamp: Time when data was read from device. Use NULL if unsupported.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_buffer(int fd, long *data, long long *timestamp)
+{
+ char str[35];
+ int count;
+
+ count = read(fd, str, sizeof(str));
+ if (!count)
+ return count;
+ if (!timestamp)
+ count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+ else
+ count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+ &data[2], timestamp);
+
+ if (count < (timestamp?4:3))
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_raw() - Read raw data.
+ * @names: Names of sysfs files.
+ * @data: Data in hardware units.
+ * @timestamp: Time when data was read from device. Use NULL if unsupported.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_raw(const struct inv_sysfs_names_s *names, long *data,
+ long long *timestamp)
+{
+ char str[40];
+ int count;
+
+ count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str);
+ if (count < 0)
+ return count;
+ if (!timestamp)
+ count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]);
+ else
+ count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1],
+ &data[2], timestamp);
+ if (count < (timestamp?4:3))
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_temperature_raw() - Read temperature.
+ * @names: Names of sysfs files.
+ * @data: Data in hardware units.
+ * @timestamp: Time when data was read from device.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data,
+ long long *timestamp)
+{
+ char str[25];
+ int count;
+
+ count = inv_sysfs_read((char*)names->temperature, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd%lld", &data[0], timestamp);
+ if (count < 2)
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_fifo_rate() - Read fifo rate.
+ * @names: Names of sysfs files.
+ * @data: Fifo rate.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data)
+{
+ char str[8];
+ int count;
+
+ count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd", data);
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_power_state() - Read power state.
+ * @names: Names of sysfs files.
+ * @data: 1 if device is on.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data)
+{
+ char str[2];
+ int count;
+
+ count = inv_sysfs_read((char*)names->power_state, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd", (short*)data);
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_scale() - Read scale.
+ * @names: Names of sysfs files.
+ * @data: 1 if device is on.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_scale(const struct inv_sysfs_names_s *names, float *data)
+{
+ char str[5];
+ int count;
+
+ count = inv_sysfs_read((char*)names->scale, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%f", data);
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_temp_scale() - Read temperature scale.
+ * @names: Names of sysfs files.
+ * @data: 1 if device is on.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data)
+{
+ char str[4];
+ int count;
+
+ count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd", data);
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_temp_offset() - Read temperature offset.
+ * @names: Names of sysfs files.
+ * @data: 1 if device is on.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data)
+{
+ char str[4];
+ int count;
+
+ count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd", data);
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+}
+
+/**
+ * inv_read_q16() - Get data as q16 fixed point.
+ * @names: Names of sysfs files.
+ * @data: 1 if device is on.
+ * @timestamp: Time when data was read from device.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_read_q16(const struct inv_sysfs_names_s *names, long *data,
+ long long *timestamp)
+{
+ int count;
+ short raw[3];
+ float scale;
+ count = inv_read_raw(names, (long*)raw, timestamp);
+ count += inv_read_scale(names, &scale);
+ data[0] = (long)(raw[0] * (65536.f / scale));
+ data[1] = (long)(raw[1] * (65536.f / scale));
+ data[2] = (long)(raw[2] * (65536.f / scale));
+ return count;
+}
+
+/**
+ * inv_read_q16() - Get temperature data as q16 fixed point.
+ * @names: Names of sysfs files.
+ * @data: 1 if device is on.
+ * @timestamp: Time when data was read from device.
+ * Returns number of bytes read or a negative error code.
+ */
+int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data,
+ long long *timestamp)
+{
+ int count = 0;
+ short raw;
+ static short scale, offset;
+ static unsigned char first_read = 1;
+
+ if (first_read) {
+ count += inv_read_temp_scale(names, &scale);
+ count += inv_read_temp_offset(names, &offset);
+ first_read = 0;
+ }
+ count += inv_read_temperature_raw(names, &raw, timestamp);
+ data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f);
+
+ return count;
+}
+
+/**
+ * inv_write_fifo_rate() - Write fifo rate.
+ * @names: Names of sysfs files.
+ * @data: Fifo rate.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data)
+{
+ return inv_sysfs_write((char*)names->fifo_rate, (long)data);
+}
+
+/**
+ * inv_write_buffer_enable() - Enable/disable buffer in /dev.
+ * @names: Names of sysfs files.
+ * @data: Fifo rate.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data)
+{
+ return inv_sysfs_write((char*)names->enable, (long)data);
+}
+
+/**
+ * inv_write_power_state() - Turn device on/off.
+ * @names: Names of sysfs files.
+ * @data: 1 to turn on.
+ * Returns number of bytes written or a negative error code.
+ */
+int inv_write_power_state(const struct inv_sysfs_names_s *names, char data)
+{
+ return inv_sysfs_write((char*)names->power_state, (long)data);
+}
+
+
diff --git a/libsensors_iio/software/core/HAL/linux/mlos_linux.c b/libsensors_iio/software/core/HAL/linux/mlos_linux.c
new file mode 100644
index 0000000..75f062e
--- /dev/null
+++ b/libsensors_iio/software/core/HAL/linux/mlos_linux.c
@@ -0,0 +1,194 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *******************************************************************************/
+
+/**
+ * @defgroup MLOS
+ * @brief OS Interface.
+ *
+ * @{
+ * @file mlos.c
+ * @brief OS Interface.
+**/
+
+/* ------------- */
+/* - Includes. - */
+/* ------------- */
+
+#include <sys/time.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdlib.h>
+
+#include "stdint_invensense.h"
+
+#include "mlos.h"
+#include <errno.h>
+
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+/**
+ * @brief Allocate space
+ * @param numBytes number of bytes
+ * @return pointer to allocated space
+**/
+void *inv_malloc(unsigned int numBytes)
+{
+ // Allocate space.
+ void *allocPtr = malloc(numBytes);
+ return allocPtr;
+}
+
+
+/**
+ * @brief Free allocated space
+ * @param ptr pointer to space to deallocate
+ * @return error code.
+**/
+inv_error_t inv_free(void *ptr)
+{
+ // Deallocate space.
+ free(ptr);
+
+ return INV_SUCCESS;
+}
+
+
+/**
+ * @brief Mutex create function
+ * @param mutex pointer to mutex handle
+ * @return error code.
+ */
+inv_error_t inv_create_mutex(HANDLE *mutex)
+{
+ int res;
+ pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t));
+ if(pm == NULL)
+ return INV_ERROR;
+
+ res = pthread_mutex_init(pm, NULL);
+ if(res == -1) {
+ free(pm);
+ return INV_ERROR_OS_CREATE_FAILED;
+ }
+
+ *mutex = (HANDLE)pm;
+
+ return INV_SUCCESS;
+}
+
+
+/**
+ * @brief Mutex lock function
+ * @param mutex Mutex handle
+ * @return error code.
+ */
+inv_error_t inv_lock_mutex(HANDLE mutex)
+{
+ int res;
+ pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
+
+ res = pthread_mutex_lock(pm);
+ if(res == -1)
+ return INV_ERROR_OS_LOCK_FAILED;
+
+ return INV_SUCCESS;
+}
+
+
+/**
+ * @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;
+
+ res = pthread_mutex_unlock(pm);
+ if(res == -1)
+ return INV_ERROR_OS_LOCK_FAILED;
+
+ return INV_SUCCESS;
+}
+
+
+/**
+ * @brief open file
+ * @param filename name of the file to open.
+ * @return error code.
+ */
+FILE *inv_fopen(char *filename)
+{
+ FILE *fp = fopen(filename,"r");
+ return fp;
+}
+
+
+/**
+ * @brief close the file.
+ * @param fp handle to file to close.
+ * @return error code.
+ */
+void inv_fclose(FILE *fp)
+{
+ fclose(fp);
+}
+
+/**
+ * @brief Close Handle
+ * @param handle handle to the resource.
+ * @return Zero if success, an error code otherwise.
+ */
+inv_error_t inv_destroy_mutex(HANDLE handle)
+{
+ int error;
+ pthread_mutex_t *pm = (pthread_mutex_t*)handle;
+ error = pthread_mutex_destroy(pm);
+ if (error) {
+ return errno;
+ }
+ free((void*) handle);
+
+ return INV_SUCCESS;}
+
+
+/**
+ * @brief Sleep function.
+ */
+void inv_sleep(int mSecs)
+{
+ usleep(mSecs*1000);
+}
+
+
+/**
+ * @brief get system's internal tick count.
+ * Used for time reference.
+ * @return current tick count.
+ */
+unsigned long inv_get_tick_count()
+{
+ struct timeval tv;
+
+ 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/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h
index 4391226..9b29695 100644
--- a/libsensors_iio/software/core/driver/include/linux/mpu.h
+++ b/libsensors_iio/software/core/driver/include/linux/mpu.h
@@ -27,8 +27,135 @@
#ifdef __KERNEL__
#include <linux/types.h>
#include <linux/ioctl.h>
+#elif defined LINUX
+#include <sys/ioctl.h>
+#include <linux/types.h>
+#else
+#include "mltypes.h"
#endif
+struct mpu_read_write {
+ /* Memory address or register address depending on ioctl */
+ __u16 address;
+ __u16 length;
+ __u8 *data;
+};
+
+enum mpuirq_data_type {
+ MPUIRQ_DATA_TYPE_MPU_DATA_READY_IRQ,
+ MPUIRQ_DATA_TYPE_MPU_FIFO_READY_IRQ,
+ MPUIRQ_DATA_TYPE_SLAVE_IRQ,
+ MPUIRQ_DATA_TYPE_PM_EVENT,
+ MPUIRQ_DATA_TYPE_NUM_TYPES,
+};
+
+/* User space PM event notification */
+#define MPU_PM_EVENT_SUSPEND_PREPARE (3)
+#define MPU_PM_EVENT_POST_SUSPEND (4)
+
+/**
+ * struct mpuirq_data - structure to report what and when
+ * @interruptcount : The number of times this IRQ has occured since open
+ * @irqtime : monotonic time of the IRQ in ns
+ * @data_type : The type of this IRQ enum mpuirq_data_type
+ * @data : Data associated with this IRQ
+ */
+struct mpuirq_data {
+ __u32 interruptcount;
+ __s64 irqtime_ns;
+ __u32 data_type;
+ __s32 data;
+};
+
+enum ext_slave_config_key {
+ /* TODO: Remove these first six. */
+ MPU_SLAVE_CONFIG_ODR_SUSPEND,
+ MPU_SLAVE_CONFIG_ODR_RESUME,
+ MPU_SLAVE_CONFIG_FSR_SUSPEND,
+ MPU_SLAVE_CONFIG_FSR_RESUME,
+ MPU_SLAVE_CONFIG_IRQ_SUSPEND,
+ MPU_SLAVE_CONFIG_IRQ_RESUME,
+ MPU_SLAVE_CONFIG_ODR,
+ MPU_SLAVE_CONFIG_FSR,
+ MPU_SLAVE_CONFIG_MOT_THS,
+ MPU_SLAVE_CONFIG_NMOT_THS,
+ MPU_SLAVE_CONFIG_MOT_DUR,
+ MPU_SLAVE_CONFIG_NMOT_DUR,
+ MPU_SLAVE_CONFIG_IRQ,
+ MPU_SLAVE_WRITE_REGISTERS,
+ MPU_SLAVE_READ_REGISTERS,
+ MPU_SLAVE_CONFIG_INTERNAL_REFERENCE,
+ /* AMI 306 specific config keys */
+ MPU_SLAVE_PARAM,
+ MPU_SLAVE_WINDOW,
+ MPU_SLAVE_READWINPARAMS,
+ MPU_SLAVE_SEARCHOFFSET,
+ /* MPU3050 and MPU6050 Keys */
+ MPU_SLAVE_INT_CONFIG,
+ MPU_SLAVE_EXT_SYNC,
+ MPU_SLAVE_FULL_SCALE,
+ MPU_SLAVE_LPF,
+ MPU_SLAVE_CLK_SRC,
+ MPU_SLAVE_DIVIDER,
+ MPU_SLAVE_DMP_ENABLE,
+ MPU_SLAVE_FIFO_ENABLE,
+ MPU_SLAVE_DMP_CFG1,
+ MPU_SLAVE_DMP_CFG2,
+ MPU_SLAVE_TC,
+ MPU_SLAVE_GYRO,
+ MPU_SLAVE_ADDR,
+ MPU_SLAVE_PRODUCT_REVISION,
+ MPU_SLAVE_SILICON_REVISION,
+ MPU_SLAVE_PRODUCT_ID,
+ MPU_SLAVE_GYRO_SENS_TRIM,
+ MPU_SLAVE_ACCEL_SENS_TRIM,
+ MPU_SLAVE_RAM,
+ /* -------------------------- */
+ MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS
+};
+
+/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */
+enum ext_slave_config_irq_type {
+ MPU_SLAVE_IRQ_TYPE_NONE,
+ MPU_SLAVE_IRQ_TYPE_MOTION,
+ MPU_SLAVE_IRQ_TYPE_DATA_READY,
+};
+
+/* Structure for the following IOCTS's
+ * MPU_CONFIG_GYRO
+ * MPU_CONFIG_ACCEL
+ * MPU_CONFIG_COMPASS
+ * MPU_CONFIG_PRESSURE
+ * MPU_GET_CONFIG_GYRO
+ * MPU_GET_CONFIG_ACCEL
+ * MPU_GET_CONFIG_COMPASS
+ * MPU_GET_CONFIG_PRESSURE
+ *
+ * @key one of enum ext_slave_config_key
+ * @len length of data pointed to by data
+ * @apply zero if communication with the chip is not necessary, false otherwise
+ * This flag can be used to select cached data or to refresh cashed data
+ * cache data to be pushed later or push immediately. If true and the
+ * slave is on the secondary bus the MPU will first enger bypass mode
+ * before calling the slaves .config or .get_config funcion
+ * @data pointer to the data to confgure or get
+ */
+struct ext_slave_config {
+ __u8 key;
+ __u16 len;
+ __u8 apply;
+ void *data;
+};
+
+enum ext_slave_type {
+ EXT_SLAVE_TYPE_GYROSCOPE,
+ EXT_SLAVE_TYPE_ACCEL,
+ EXT_SLAVE_TYPE_COMPASS,
+ EXT_SLAVE_TYPE_PRESSURE,
+ /*EXT_SLAVE_TYPE_TEMPERATURE */
+
+ EXT_SLAVE_NUM_TYPES
+};
enum secondary_slave_type {
SECONDARY_SLAVE_TYPE_NONE,
SECONDARY_SLAVE_TYPE_ACCEL,
@@ -77,6 +204,127 @@
};
#define INV_PROD_KEY(ver, rev) (ver * 100 + rev)
+
+enum ext_slave_endian {
+ EXT_SLAVE_BIG_ENDIAN,
+ EXT_SLAVE_LITTLE_ENDIAN,
+ EXT_SLAVE_FS8_BIG_ENDIAN,
+ EXT_SLAVE_FS16_BIG_ENDIAN,
+};
+
+enum ext_slave_bus {
+ EXT_SLAVE_BUS_INVALID = -1,
+ EXT_SLAVE_BUS_PRIMARY = 0,
+ EXT_SLAVE_BUS_SECONDARY = 1
+};
+
+
+/**
+ * struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050
+ * slave devices
+ *
+ * @type: the type of slave device based on the enum ext_slave_type
+ * definitions.
+ * @irq: the irq number attached to the slave if any.
+ * @adapt_num: the I2C adapter number.
+ * @bus: the bus the slave is attached to: enum ext_slave_bus
+ * @address: the I2C slave address of the slave device.
+ * @orientation: the mounting matrix of the device relative to MPU.
+ * @irq_data: private data for the slave irq handler
+ * @private_data: additional data, user customizable. Not touched by the MPU
+ * driver.
+ *
+ * The orientation matricies are 3x3 rotation matricies
+ * that are applied to the data to rotate from the mounting orientation to the
+ * platform orientation. The values must be one of 0, 1, or -1 and each row and
+ * column should have exactly 1 non-zero value.
+ */
+struct ext_slave_platform_data {
+ __u8 type;
+ __u32 irq;
+ __u32 adapt_num;
+ __u32 bus;
+ __u8 address;
+ __s8 orientation[9];
+ void *irq_data;
+ void *private_data;
+};
+
+struct fix_pnt_range {
+ __s32 mantissa;
+ __s32 fraction;
+};
+
+static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
+{
+ return (long)(rng.mantissa * 1000 + rng.fraction / 10);
+}
+
+struct ext_slave_read_trigger {
+ __u8 reg;
+ __u8 value;
+};
+
+/**
+ * struct ext_slave_descr - Description of the slave device for programming.
+ *
+ * @suspend: function pointer to put the device in suspended state
+ * @resume: function pointer to put the device in running state
+ * @read: function that reads the device data
+ * @init: function used to preallocate memory used by the driver
+ * @exit: function used to free memory allocated for the driver
+ * @config: function used to configure the device
+ * @get_config:function used to get the device's configuration
+ *
+ * @name: text name of the device
+ * @type: device type. enum ext_slave_type
+ * @id: enum ext_slave_id
+ * @read_reg: starting register address to retrieve data.
+ * @read_len: length in bytes of the sensor data. Typically 6.
+ * @endian: byte order of the data. enum ext_slave_endian
+ * @range: full scale range of the slave ouput: struct fix_pnt_range
+ * @trigger: If reading data first requires writing a register this is the
+ * data to write.
+ *
+ * Defines the functions and information about the slave the mpu3050 and
+ * mpu6050 needs to use the slave device.
+ */
+struct ext_slave_descr {
+ int (*init) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata);
+ int (*exit) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata);
+ int (*suspend) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata);
+ int (*resume) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata);
+ int (*read) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata,
+ __u8 *data);
+ int (*config) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata,
+ struct ext_slave_config *config);
+ int (*get_config) (void *mlsl_handle,
+ struct ext_slave_descr *slave,
+ struct ext_slave_platform_data *pdata,
+ struct ext_slave_config *config);
+
+ char *name;
+ __u8 type;
+ __u8 id;
+ __u8 read_reg;
+ __u8 read_len;
+ __u8 endian;
+ struct fix_pnt_range range;
+ struct ext_slave_read_trigger *trigger;
+};
+
/**
* struct mpu_platform_data - Platform data for the mpu driver
* @int_config: Bits [7:3] of the int config register.
@@ -86,7 +334,6 @@
* @sec_slave_id: id of the secondary slave device
* @secondary_i2c_address: secondary device's i2c address
* @secondary_orientation: secondary device's orientation matrix
- * @key: key for MPL library.
*
* Contains platform specific information on how to configure the MPU3050 to
* work on this platform. The orientation matricies are 3x3 rotation matricies
diff --git a/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h
index 97e5b2f..74c49f3 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,10 +56,6 @@
extern "C" {
#endif
-#if defined ANDROID_JELLYBEAN
-#define LOG ALOG
-#endif
-
/* --------------------------------------------------------------------- */
/*
@@ -291,7 +287,7 @@
#ifndef MPL_LOG_PRI
#ifdef ANDROID
#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- ALOG(priority, tag, fmt, ##__VA_ARGS__)
+ LOG(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/CMakeLists.txt b/libsensors_iio/software/core/mllite/CMakeLists.txt
new file mode 100644
index 0000000..8650553
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/CMakeLists.txt
@@ -0,0 +1,39 @@
+## CMakeLists for mlsdk/mldmp
+
+project(mldmp C)
+
+if (NOT CMAKE_BUILD_ENGINEERING)
+
+# # just copy the library from mldmp/mpl/$PLATFORM to mldmp/
+# if (CMAKE_SYSTEM_NAME MATCHES Windows)
+# execute_process(
+# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib
+# )
+# elseif (CMAKE_SYSTEM_NAME MATCHES Android)
+# execute_process(
+# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
+# )
+# elseif (CMAKE_SYSTEM_NAME MATCHES Linux)
+# message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a")
+# execute_process(
+# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
+# )
+# elseif (CMAKE_SYSTEM_NAME MATCHES Catalina)
+# execute_process(
+# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a
+# )
+# endif()
+# # better way that doesn't work for now
+# # add_custom_command(
+# # TARGET mpl
+# # PRE_BUILD
+# # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR}
+# # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}"
+# # )
+
+else (NOT CMAKE_BUILD_ENGINEERING)
+
+ set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR})
+ include(Engineering)
+
+endif (NOT CMAKE_BUILD_ENGINEERING)
diff --git a/libsensors_iio/software/core/mllite/Engineering.cmake b/libsensors_iio/software/core/mllite/Engineering.cmake
new file mode 100644
index 0000000..42f2429
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/Engineering.cmake
@@ -0,0 +1,150 @@
+## Engineering CMakeLists for software/core/mllite
+
+include_directories(
+ .
+ ${SOLUTION_SOURCE_DIR}/core/mpl
+ ${SOLUTION_SOURCE_DIR}/core/HAL
+ ${SOLUTION_SOURCE_DIR}/driver/include
+)
+
+add_definitions (
+ -DINV_INCLUDE_LEGACY_HEADERS
+)
+
+if (CMAKE_SYSTEM_NAME MATCHES Android)
+
+ include_directories(
+ ${SOLUTION_SOURCE_DIR}/core/mllite/linux
+ ${SOLUTION_SOURCE_DIR}/driver/include/linux
+ )
+ add_definitions(
+ -DLINUX
+ -D_REENTRANT
+ )
+ set (HEADERS
+ ${HEADERS}
+ linux/mlos.h
+ linux/ml_stored_data.h
+ linux/ml_load_dmp.h
+ linux/ml_sysfs_helper.h
+ )
+ set (SOURCES
+ ${SOURCES}
+ linux/mlos_linux.c
+ linux/ml_stored_data.c
+ linux/ml_load_dmp.c
+ linux/ml_sysfs_helper.c
+ )
+
+elseif (CMAKE_SYSTEM_NAME MATCHES Linux)
+
+ add_definitions(
+ -DLINUX
+ -D_REENTRANT
+ )
+ set (HEADERS
+ ${HEADERS}
+ )
+ set (SOURCES
+ ${SOURCES}
+ )
+
+elseif (CMAKE_SYSTEM_NAME MATCHES Windows)
+
+ add_definitions(
+ -DAIO
+ -DUART_COM
+ -D_CRT_SECURE_NO_WARNINGS
+ -D_CRT_SECURE_NO_DEPRECATE
+ )
+ set (HEADERS
+ ${HEADERS}
+ )
+ set (SOURCES
+ ${SOURCES}
+ )
+
+endif ()
+
+set (HEADERS
+ ${HEADERS}
+ data_builder.h
+ hal_outputs.h
+ message_layer.h
+ ml_math_func.h
+ mpl.h
+ results_holder.h
+ start_manager.h
+ storage_manager.h
+)
+set (SOURCES
+ ${SOURCES}
+ data_builder.c
+ hal_outputs.c
+ message_layer.c
+ ml_math_func.c
+ mpl.c
+ results_holder.c
+ start_manager.c
+ storage_manager.c
+)
+
+# coveniently add this file to the resources for easy access within IDEs
+set (RESOURCES
+ Engineering.cmake
+)
+
+if (CMAKE_TESTING_SUPPORT)
+
+ message("Including Testing support")
+ include_directories(
+ ${SOLUTION_SOURCE_DIR}/mltools/debugsupport
+ )
+ add_definitions(
+ -DSELF_VERIFICATION
+ )
+ set(
+ HEADERS
+ ${HEADERS}
+ ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h
+ )
+ set(
+ SOURCES
+ ${SOURCES}
+ ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c
+ )
+
+endif ()
+
+############
+## TARGET ##
+############
+add_library(
+ mllite STATIC
+ ${SOURCES}
+ ${HEADERS}
+ ${RESOURCES}
+)
+set_target_properties(
+ mllite
+ PROPERTIES CLEAN_DIRECT_OUTPUT 1
+)
+
+if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)")
+
+ add_library(
+ mllite_shared SHARED
+ ${SOURCES}
+ ${HEADERS}
+ ${RESOURCES}
+ )
+ FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite")
+
+ install (
+ TARGETS mllite_shared
+ DESTINATION lib
+ )
+
+endif ()
+
+
diff --git a/libsensors_iio/software/core/mllite/build/android/libmllite.so b/libsensors_iio/software/core/mllite/build/android/libmllite.so
deleted file mode 100644
index acd25a7..0000000
--- a/libsensors_iio/software/core/mllite/build/android/libmllite.so
+++ /dev/null
Binary files differ
diff --git a/libsensors_iio/software/core/mllite/build/android/shared.mk b/libsensors_iio/software/core/mllite/build/android/shared.mk
index 1418450..2ee2e20 100644
--- a/libsensors_iio/software/core/mllite/build/android/shared.mk
+++ b/libsensors_iio/software/core/mllite/build/android/shared.mk
@@ -15,7 +15,6 @@
include $(INV_ROOT)/software/build/android/common.mk
CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += $(ANDROID_COMPILE)
CFLAGS += -Wall
CFLAGS += -fpic
CFLAGS += -nostdlib
@@ -45,7 +44,12 @@
LFLAGS += $(CMDLINE_LFLAGS)
LFLAGS += -shared
LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -Wl,-shared,-Bsymbolic
+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/mllite/build/android/static.mk b/libsensors_iio/software/core/mllite/build/android/static.mk
new file mode 100644
index 0000000..6ad45de
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/build/android/static.mk
@@ -0,0 +1,110 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+OBJFOLDER = $(CURDIR)/obj
+
+CROSS = arm-none-linux-gnueabi-
+COMP= $(CROSS)gcc
+LINK= $(CROSS)ar cr
+
+MLLITE_DIR = $(MLSDK_ROOT)/mllite
+MPL_DIR = $(MLSDK_ROOT)/mldmp
+MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
+
+include $(MLSDK_ROOT)/Android-common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall -fpic
+CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT -DLINUX -DANDROID
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MLPLATFORM_DIR)/include
+CFLAGS += -I$(MLSDK_ROOT)/mlutils
+CFLAGS += -I$(MLSDK_ROOT)/mlapps/common
+CFLAGS += $(MLSDK_INCLUDES)
+CFLAGS += $(MLSDK_DEFINES)
+
+VPATH += $(MLLITE_DIR)
+VPATH += $(MLSDK_ROOT)/mlutils
+VPATH += $(MLLITE_DIR)/accel
+VPATH += $(MLLITE_DIR)/compass
+VPATH += $(MLLITE_DIR)/pressure
+VPATH += $(MLLITE_DIR)/mlapps/common
+
+####################################################################################################
+## sources
+
+ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT)
+
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c
+ML_SOURCES += $(MLLITE_DIR)/accel.c
+ML_SOURCES += $(MLLITE_DIR)/compass.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c
+ML_SOURCES += $(MLLITE_DIR)/key0_96.c
+ML_SOURCES += $(MLLITE_DIR)/pressure.c
+ML_SOURCES += $(MLLITE_DIR)/ml.c
+ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c
+ML_SOURCES += $(MLLITE_DIR)/ml_init.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c
+ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c
+ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c
+ML_SOURCES += $(MLLITE_DIR)/mldl.c
+ML_SOURCES += $(MLLITE_DIR)/mldmp.c
+ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c
+ML_SOURCES += $(MLLITE_DIR)/mlstates.c
+ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c
+ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c
+ifeq ($(MPU_NAME),MPU3050)
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c
+else
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c
+endif
+
+ML_OBJS := $(addsuffix .o,$(ML_SOURCES))
+ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall
+
+all: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME)
+ @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n")
+ $(LINK) $(LIBRARY) $(ML_OBJS_DST)
+ $(CROSS)ranlib $(LIBRARY)
+
+$(OBJFOLDER) :
+ @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+ mkdir obj
+
+$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME)
+ @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+ $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $<
+
+clean :
+ rm -fR $(OBJFOLDER)
+
+cleanall :
+ rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c
index 48993bc..f70be7c 100644
--- a/libsensors_iio/software/core/mllite/data_builder.c
+++ b/libsensors_iio/software/core/mllite/data_builder.c
@@ -18,14 +18,11 @@
#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"
#include "storage_manager.h"
#include "message_layer.h"
-#include "results_holder.h"
#include "log.h"
#undef MPL_LOG_TAG
@@ -51,10 +48,6 @@
/** Temperature when accel bias was stored. */
long accel_temp;
long gyro_temp_slope[3];
- /** Sensor Accuracy */
- int gyro_accuracy;
- int accel_accuracy;
- int compass_accuracy;
};
struct inv_data_builder_t {
@@ -76,7 +69,7 @@
static struct inv_sensor_cal_t sensors;
/** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53395
+#define INV_DB_SAVE_KEY 53394
#ifdef INV_PLAYBACK_DBG
@@ -105,14 +98,6 @@
static inv_error_t inv_db_load_func(const unsigned char *data)
{
memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
- // copy in the saved accuracy in the actual sensors accuracy
- sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
- sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
- sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
- // TODO
- if (sensors.compass.accuracy == 3) {
- inv_set_compass_bias_found(1);
- }
return INV_SUCCESS;
}
@@ -177,7 +162,119 @@
sensor->sensitivity = sensitivity;
sensor->orientation = orientation;
}
+void inv_get_quaternion_transformation(int orientation, long *q)
+{
+ long s = 759250125; // sqrt(.5)*2^30
+ switch (orientation)
+ {
+ case 0x70:
+ q[0] = 759250125;
+ q[1] = 759250125;
+ q[2] = 0;
+ q[3] = 0;
+ break;
+ case 0x10a:
+ q[0] = 759250125;
+ q[1] = 0;
+ q[2] = 759250125;
+ q[3] = 0;
+ break;
+ case 0x85:
+ q[0] = 759250125;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = 759250125;
+ break;
+ case 0x181:
+ q[0] = 0;
+ q[1] = 759250125;
+ q[2] = 759250125;
+ q[3] = 0;
+ break;
+ case 0x2a:
+ q[0] = 0;
+ q[1] = 759250125;
+ q[2] = 0;
+ q[3] = 759250125;
+ break;
+ case 0x54:
+ q[0] = 0;
+ q[1] = 0;
+ q[2] = 759250125;
+ q[3] = 759250125;
+ break;
+ case 0x150:
+ q[0] = 759250125;
+ q[1] = -759250125;
+ q[2] = 0;
+ q[3] = 0;
+ break;
+ case 0xe:
+ q[0] = 759250125;
+ q[1] = 0;
+ q[2] = -759250125;
+ q[3] = 0;
+ break;
+ case 0xa1:
+ q[0] = 759250125;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = -759250125;
+ break;
+ case 0x1a5:
+ q[0] = 0;
+ q[1] = 759250125;
+ q[2] = -759250125;
+ q[3] = 0;
+ break;
+ case 0x12e:
+ q[0] = 0;
+ q[1] = 759250125;
+ q[2] = 0;
+ q[3] = -759250125;
+ break;
+ case 0x174:
+ q[0] = 0;
+ q[1] = 0;
+ q[2] = 759250125;
+ q[3] = -759250125;
+ break;
+
+
+ case 0x88:
+ q[0] = 1073741824;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = 0;
+ break;
+
+
+ case 0x1a8:
+ q[0] = 0;
+ q[1] = 1073741824;
+ q[2] = 0;
+ q[3] = 0;
+ break;
+
+ case 0x18c:
+ q[0] = 0;
+ q[1] = 0;
+ q[2] = 1073741824;
+ q[3] = 0;
+ break;
+
+ case 0xac:
+ q[0] = 0;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = 1073741824;
+ break;
+
+ default:
+ break;
+ }
+}
/** Sets the Orientation and Sensitivity of the gyro data.
* @param[in] orientation A scalar defining the transformation from chip mounting
* to the body frame. The function inv_orientation_matrix_to_scalar()
@@ -257,22 +354,6 @@
sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
}
}
-
-void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
-{
- *sample_rate_ms = sensors.gyro.sample_rate_ms;
-}
-
-void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
-{
- *sample_rate_ms = sensors.accel.sample_rate_ms;
-}
-
-void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
-{
- *sample_rate_ms = sensors.compass.sample_rate_ms;
-}
-
/** Set Quat Sample rate in micro seconds.
* @param[in] sample_rate_us Set Quat Sample rate in us
*/
@@ -417,7 +498,7 @@
}
/** Takes raw data stored in the sensor, removes bias, and converts it to
-* calibrated data in the body frame. Also store raw data for body frame.
+* calibrated data in the body frame.
* @param[in,out] sensor structure to modify
* @param[in] bias bias in the mounting frame, in hardware units scaled by
* 2^16. Length 3.
@@ -427,17 +508,15 @@
long raw32[3];
// Convert raw to calibrated
- raw32[0] = (long)sensor->raw[0] << 15;
- raw32[1] = (long)sensor->raw[1] << 15;
- raw32[2] = (long)sensor->raw[2] << 15;
+ raw32[0] = (long)sensor->raw[0] << 16;
+ raw32[1] = (long)sensor->raw[1] << 16;
+ raw32[2] = (long)sensor->raw[2] << 16;
- inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
+ raw32[0] -= bias[0];
+ raw32[1] -= bias[1];
+ raw32[2] -= bias[2];
- raw32[0] -= bias[0] >> 1;
- raw32[1] -= bias[1] >> 1;
- raw32[2] -= bias[2] >> 1;
-
- inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
+ inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated);
sensor->status |= INV_CALIBRATED;
}
@@ -460,7 +539,6 @@
inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
}
sensors.compass.accuracy = accuracy;
- inv_data_builder.save.compass_accuracy = accuracy;
inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
}
@@ -488,17 +566,6 @@
}
}
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 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);
}
@@ -523,8 +590,6 @@
inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
}
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);
}
@@ -542,8 +607,6 @@
}
}
sensors.gyro.accuracy = accuracy;
- inv_data_builder.save.gyro_accuracy = accuracy;
-
/* TODO: What should we do if there's no temperature data? */
if (sensors.temp.calibrated[0])
inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
@@ -624,7 +687,6 @@
sensors.accel.calibrated[2] = accel[2];
sensors.accel.status |= INV_CALIBRATED;
sensors.accel.accuracy = status & 3;
- inv_data_builder.save.accel_accuracy = status & 3;
}
sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
sensors.accel.timestamp_prev = sensors.accel.timestamp;
@@ -695,7 +757,6 @@
sensors.compass.calibrated[2] = compass[2];
sensors.compass.status |= INV_CALIBRATED;
sensors.compass.accuracy = status & 3;
- inv_data_builder.save.compass_accuracy = status & 3;
}
sensors.compass.timestamp_prev = sensors.compass.timestamp;
sensors.compass.timestamp = timestamp;
@@ -1016,22 +1077,6 @@
}
}
-/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
- * @param[out] data Gyro Data where 1 dps = 2^16
- * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
- * @param[out] timestamp The timestamp of the data sample.
-*/
-void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
-{
- memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
- if (timestamp != NULL) {
- *timestamp = sensors.gyro.timestamp;
- }
- if (accuracy != NULL) {
- *accuracy = sensors.gyro.accuracy;
- }
-}
-
/** Get's latest gyro data.
* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
*/
diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h
index 9aa46e6..b2d0881 100644
--- a/libsensors_iio/software/core/mllite/data_builder.h
+++ b/libsensors_iio/software/core/mllite/data_builder.h
@@ -56,7 +56,6 @@
#define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850
#define INV_PRIORITY_HAL_OUTPUTS 900
#define INV_PRIORITY_GLYPH 950
-#define INV_PRIORITY_SHAKE 975
#define INV_PRIORITY_SM 1000
struct inv_single_sensor_t {
@@ -70,8 +69,6 @@
int orientation;
/** The raw data in raw data units in the mounting frame */
short raw[3];
- /** Raw data in body frame */
- long raw_scaled[3];
/** Calibrated data */
long calibrated[3];
long sensitivity;
@@ -163,10 +160,6 @@
void inv_set_accel_bandwidth(int bandwidth_hz);
void inv_set_compass_bandwidth(int bandwidth_hz);
-void inv_get_gyro_sample_rate_ms(long *sample_rate_ms);
-void inv_get_accel_sample_rate_ms(long *sample_rate_ms);
-void inv_get_compass_sample_rate_ms(long *sample_rate_ms);
-
inv_error_t inv_register_data_cb(inv_error_t (*func)
(struct inv_sensor_cal_t * data), int priority,
int sensor_type);
@@ -188,7 +181,6 @@
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);
@@ -205,7 +197,6 @@
void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
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);
void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
void inv_get_gyro(long *gyro);
diff --git a/libsensors_iio/software/core/mllite/dmpconfig.txt b/libsensors_iio/software/core/mllite/dmpconfig.txt
new file mode 100644
index 0000000..4643ed5
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/dmpconfig.txt
@@ -0,0 +1,3 @@
+major version = 1
+minor version = 0
+startAddr = 0
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c
index 6d457db..1cd3b81 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.c
+++ b/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -4,19 +4,16 @@
See included License.txt for License information.
$
*/
-
+
/**
* @defgroup HAL_Outputs hal_outputs
* @brief Motion Library - HAL Outputs
* 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"
@@ -26,22 +23,17 @@
#include "results_holder.h"
struct hal_output_t {
- int accuracy_mag; /**< Compass accuracy */
-// int accuracy_gyro; /**< Gyro Accuracy */
-// int accuracy_accel; /**< Accel Accuracy */
- int accuracy_quat; /**< quat Accuracy */
-
+ int accuracy_mag; /**< Compass accuracy */
+ int accuracy_gyro; /**< Gyro Accuracy */
+ int accuracy_accel; /**< Accel Accuracy */
inv_time_t nav_timestamp;
inv_time_t gam_timestamp;
-// inv_time_t accel_timestamp;
- inv_time_t mag_timestamp;
+ inv_time_t accel_timestamp;
long nav_quat[4];
int gyro_status;
int accel_status;
int compass_status;
int nine_axis_status;
- inv_biquad_filter_t lp_filter[3];
- float compass_float[3];
};
static struct hal_output_t hal_out;
@@ -111,7 +103,7 @@
long gravity[3];
int status;
- *accuracy = (int8_t) hal_out.accuracy_quat;
+ *accuracy = hal_out.accuracy_mag;
*timestamp = hal_out.nav_timestamp;
inv_get_gravity(gravity);
values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
@@ -124,11 +116,7 @@
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.
+/** Gyroscope 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.
* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
@@ -138,6 +126,9 @@
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;
@@ -152,30 +143,6 @@
return status;
}
-/** Gyroscope raw 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.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_gyro().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gyro[3];
- int status;
-
- inv_get_gyro_set_raw(gyro, accuracy, timestamp);
- values[0] = gyro[0] * GYRO_CONVERSION;
- values[1] = gyro[1] * GYRO_CONVERSION;
- values[2] = gyro[2] * GYRO_CONVERSION;
- if (hal_out.gyro_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
/**
* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
* The rotation vector represents the orientation of the device as a combination
@@ -203,7 +170,7 @@
int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
inv_time_t * timestamp)
{
- *accuracy = (int8_t) hal_out.accuracy_quat;
+ *accuracy = hal_out.accuracy_mag;
*timestamp = hal_out.nav_timestamp;
if (hal_out.nav_quat[0] >= 0) {
@@ -236,54 +203,19 @@
int status;
/* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
* So this is: 1 / 2^16*/
-//#define COMPASS_CONVERSION 1.52587890625e-005f
- int i;
-
- *timestamp = hal_out.mag_timestamp;
- *accuracy = (int8_t) hal_out.accuracy_mag;
-
- for (i=0; i<3; i++) {
- values[i] = hal_out.compass_float[i];
- }
+#define COMPASS_CONVERSION 1.52587890625e-005f
+ long compass[3];
+ inv_get_compass_set(compass, accuracy, timestamp);
+ values[0] = compass[0] * COMPASS_CONVERSION;
+ values[1] = compass[1] * COMPASS_CONVERSION;
+ values[2] = compass[2] * COMPASS_CONVERSION;
if (hal_out.compass_status & INV_NEW_DATA)
status = 1;
else
status = 0;
- hal_out.compass_status = 0;
return status;
}
-static void inv_get_rotation(float r[3][3])
-{
- long rot[9];
- float conv = 1.f / (1L<<30);
-
- inv_quaternion_to_rotation(hal_out.nav_quat, rot);
- r[0][0] = rot[0]*conv;
- r[0][1] = rot[1]*conv;
- r[0][2] = rot[2]*conv;
- r[1][0] = rot[3]*conv;
- r[1][1] = rot[4]*conv;
- r[1][2] = rot[5]*conv;
- r[2][0] = rot[6]*conv;
- r[2][1] = rot[7]*conv;
- r[2][2] = rot[8]*conv;
-}
-
-static void google_orientation(float *g)
-{
- float rad2deg = (float)(180.0 / M_PI);
- float R[3][3];
-
- inv_get_rotation(R);
-
- g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
- g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
- g[2] = asinf ( R[2][0]) * rad2deg;
- if (g[0] < 0)
- g[0] += 360;
-}
-
/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
* @param[out] values Length 3, Degrees.<br>
@@ -309,29 +241,78 @@
int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
inv_time_t * timestamp)
{
- *accuracy = (int8_t) hal_out.accuracy_quat;
+ long t1, t2, t3;
+ long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
+
+ *accuracy = hal_out.accuracy_mag;
*timestamp = hal_out.nav_timestamp;
- google_orientation(values);
+ q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]);
+ q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]);
+ q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]);
+ q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]);
+ q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]);
+ q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]);
+ q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]);
+ q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]);
+ q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]);
+ q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]);
+
+ /* X component of the Ybody axis in World frame */
+ t1 = q12 - q03;
+
+ /* Y component of the Ybody axis in World frame */
+ t2 = q22 + q00 - (1L << 30);
+ values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
+ if (values[0] < 0)
+ values[0] += 360;
+
+ /* Z component of the Ybody axis in World frame */
+ t3 = q23 + q01;
+ values[1] =
+ -atan2f((float) t3,
+ sqrtf((float) t1 * t1 +
+ (float) t2 * t2)) * 180.f / (float) M_PI;
+ /* Z component of the Zbody axis in World frame */
+ t2 = q33 + q00 - (1L << 30);
+ if (t2 < 0) {
+ if (values[1] >= 0)
+ values[1] = 180.f - values[1];
+ else
+ values[1] = -180.f - values[1];
+ }
+
+ /* X component of the Xbody axis in World frame */
+ t1 = q11 + q00 - (1L << 30);
+ /* Y component of the Xbody axis in World frame */
+ t2 = q12 + q03;
+ /* Z component of the Xbody axis in World frame */
+ t3 = q13 - q02;
+ //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI;
+
+ values[2] =
+ -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
+ 180.f / (float) M_PI - 90);
+ if (values[2] >= 90)
+ values[2] = 180 - values[2];
+
+ if (values[2] < -90)
+ values[2] = -180 - values[2];
return hal_out.nine_axis_status;
}
/** Main callback to generate HAL outputs. Typically not called by library users.
-* @param[in] sensor_cal Input variable to take sensor data whenever there is new
+* @param[in] sensor_cal Input variable to take sensor data whenever there is new
* sensor data.
* @return Returns INV_SUCCESS if successful or an error code if not.
*/
inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
{
int use_sensor = 0;
- long sr = 1000;
- long compass[3];
- int8_t accuracy;
- int i;
+ long sr;
(void) sensor_cal;
-
- inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
+ inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag,
&hal_out.nav_timestamp);
hal_out.gyro_status = sensor_cal->gyro.status;
hal_out.accel_status = sensor_cal->accel.status;
@@ -355,65 +336,26 @@
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;
+ hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
break;
case 1:
hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->accel.timestamp;
+ hal_out.nav_timestamp = sensor_cal->accel.timestamp;
break;
case 2:
hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->compass.timestamp;
+ hal_out.nav_timestamp = sensor_cal->compass.timestamp;
break;
case 3:
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
+ hal_out.nav_timestamp = sensor_cal->quat.timestamp;
break;
}
- /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
- * So this is: 1 / 2^16*/
- #define COMPASS_CONVERSION 1.52587890625e-005f
-
- inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
- hal_out.accuracy_mag = (int ) accuracy;
-
- for (i=0; i<3; i++) {
- if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
- INV_NEW_DATA ) {
- // set the state variables to match output with input
- inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);
- }
-
- if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
- (INV_NEW_DATA | INV_RAW_DATA) ) {
-
- hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],
- (float ) compass[i]) * COMPASS_CONVERSION;
-
- } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) {
- hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;
- }
-
- }
return INV_SUCCESS;
}
@@ -440,9 +382,6 @@
INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
return result;
}
-/* file name: lowPassFilterCoeff_1_6.c */
-float compass_low_pass_filter_coeff[5] =
-{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
/** Initializes hal outputs class. This is called automatically by the
* enable function. It may be called any time the feature is enabled, but
@@ -451,12 +390,7 @@
*/
inv_error_t inv_init_hal_outputs(void)
{
- int i;
memset(&hal_out, 0, sizeof(hal_out));
- for (i=0; i<3; i++) {
- inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
- }
-
return INV_SUCCESS;
}
@@ -466,8 +400,8 @@
inv_error_t inv_enable_hal_outputs(void)
{
inv_error_t result;
-
- // don't need to check the result for inv_init_hal_outputs
+
+ // don't need to check the result for inv_init_hal_outputs
// since it's always INV_SUCCESS
inv_init_hal_outputs();
@@ -481,7 +415,7 @@
{
inv_error_t result;
- inv_stop_hal_outputs(); // Ignore error if we have already stopped this
+ inv_stop_hal_outputs(); // Ignore error if we have already stopped this
result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
return result;
}
diff --git a/libsensors_iio/software/core/mllite/hal_outputs.h b/libsensors_iio/software/core/mllite/hal_outputs.h
index 85e88f3..ed4cb65 100644
--- a/libsensors_iio/software/core/mllite/hal_outputs.h
+++ b/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -19,8 +19,6 @@
inv_time_t * timestamp);
int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
inv_time_t * timestamp);
- int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
inv_time_t * timestamp);
int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
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 bdf350f..f0f078c 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
@@ -30,10 +30,10 @@
#define LOADDMP_LOG MPL_LOGI
#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
-#define DMP_CODE_SIZE 3065
+#define DMP_CODE_SIZE 3060
static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
- /* bank # 0 */
+ /* bank # 0 */
0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
@@ -69,15 +69,15 @@
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, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x01, 0x00, 0x44, 0x00, 0x05, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 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,
- 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,
+ 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, 0x47, 0x78, 0xa2,
0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
@@ -106,9 +106,9 @@
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, 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,
+ 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,
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,
@@ -212,32 +212,32 @@
0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
- 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
- 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09,
- 0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b,
- 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
- 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba,
- 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
- 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80,
- 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79,
- 0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85,
+ 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x8b, 0x93, 0xf2, 0x02, 0x02, 0xd1, 0xab, 0xda,
+ 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
+ 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
+ 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
+ 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 0xa3, 0xa3,
+ 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xd8, 0xd8,
+ 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80,
+ 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 0x2c, 0x87,
+ 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79,
/* bank # 11 */
- 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85,
- 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10,
- 0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8,
- 0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c,
- 0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89,
- 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95,
- 0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39,
- 0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda,
- 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda,
- 0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1,
- 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0,
- 0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde,
- 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96,
- 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c,
- 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35,
- 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
+ 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28,
+ 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2,
+ 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 0x80, 0xd9,
+ 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 0x2c, 0xd8,
+ 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c,
+ 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 0x0c, 0xd8,
+ 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 0xff, 0xd8,
+ 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8,
+ 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x87, 0x31,
+ 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0xa8, 0x82,
+ 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 0xf1, 0xa6,
+ 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0x2a,
+ 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c,
+ 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde,
+ 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9,
+ 0x00, 0xd8, 0xf1, 0xff
};
#define DMP_VERSION (dmpMemory)
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 b4c4249..c5cf2e6 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
@@ -21,12 +21,13 @@
#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
@@ -37,40 +38,26 @@
#define STORECAL_LOG MPL_LOGI
#define LOADCAL_LOG MPL_LOGI
-inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
+inv_error_t inv_read_cal(unsigned char *cal, size_t len)
{
FILE *fp;
+ int bytesRead;
inv_error_t result = INV_SUCCESS;
- size_t fsize;
fp = fopen(MLCAL_FILE,"rb");
if (fp == NULL) {
MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
return INV_ERROR_FILE_OPEN;
}
-
- // obtain file size
- fseek (fp, 0 , SEEK_END);
- fsize = ftell (fp);
- rewind (fp);
-
- *calData = (unsigned char *)inv_malloc(fsize);
- if (*calData==NULL) {
- MPL_LOGE("Could not allocate buffer of %d bytes - "
- "aborting\n", fsize);
- fclose(fp);
- return INV_ERROR_MEMORY_EXAUSTED;
- }
-
- *bytesRead = fread(*calData, 1, fsize, fp);
- if (*bytesRead != fsize) {
- MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
- *bytesRead, fsize);
+ bytesRead = fread(cal, 1, len, fp);
+ if (bytesRead != len) {
+ MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
+ bytesRead, len);
result = INV_ERROR_FILE_READ;
goto read_cal_end;
}
else {
- MPL_LOGI("Bytes read = %d", *bytesRead);
+ MPL_LOGI("Bytes read = %d", bytesRead);
}
read_cal_end:
@@ -274,18 +261,31 @@
*/
inv_error_t inv_load_calibration(void)
{
- unsigned char *calData= NULL;
+ unsigned char *calData;
inv_error_t result = 0;
- size_t bytesRead = 0;
+ size_t length;
- result = inv_read_cal(&calData, &bytesRead);
+ inv_get_mpl_state_size(&length);
+ if (length <= 0) {
+ MPL_LOGE("Could not get file calibration length - "
+ "error %d - aborting\n", result);
+ return result;
+ }
+
+ calData = (unsigned char *)inv_malloc(length);
+ if (!calData) {
+ MPL_LOGE("Could not allocate buffer of %d bytes - "
+ "aborting\n", length);
+ return INV_ERROR_MEMORY_EXAUSTED;
+ }
+
+ result = inv_read_cal(calData, length);
if(result != INV_SUCCESS) {
MPL_LOGE("Could not load cal file - "
"aborting\n");
- goto free_mem_n_exit;
}
- result = inv_load_mpl_states(calData, bytesRead);
+ result = inv_load_mpl_states(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not load the calibration data - "
"error %d - aborting\n", result);
@@ -294,7 +294,7 @@
free_mem_n_exit:
inv_free(calData);
- return result;
+ return INV_SUCCESS;
}
/**
@@ -345,7 +345,7 @@
free_mem_n_exit:
inv_free(calData);
- return result;
+ return INV_SUCCESS;
}
/**
diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
index 115b34c..336f081 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
+++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
@@ -36,7 +36,7 @@
/*
Internal APIs
*/
-inv_error_t inv_read_cal(unsigned char **, size_t *);
+inv_error_t inv_read_cal(unsigned char *cal, size_t len);
inv_error_t inv_write_cal(unsigned char *cal, size_t len);
inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len);
inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len);
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 eca57c3..5636a02 100644
--- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
+++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
@@ -5,6 +5,7 @@
#include <ctype.h>
#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu"
+#define CHIP_NUM 4
enum PROC_SYSFS_CMD {
CMD_GET_SYSFS_PATH,
CMD_GET_DMP_PATH,
@@ -14,13 +15,7 @@
CMD_GET_DEVICE_NODE
};
static char sysfs_path[100];
-static char *chip_name[] = {
- "ITG3500",
- "MPU6050",
- "MPU9150",
- "MPU3050",
- "MPU6500"
-};
+static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"};
static int chip_ind;
static int initialized =0;
static int status = 0;
@@ -32,8 +27,6 @@
#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
#define FORMAT_TYPE_FILE "%s_type"
-#define CHIP_NUM ARRAY_SIZE(chip_name)
-
static const char *iio_dir = "/sys/bus/iio/devices/";
/**
@@ -100,7 +93,7 @@
*/
static int parsing_proc_input(int mode, char *name){
const char input[] = "/proc/bus/input/devices";
- char line[4096], d;
+ char line[100], d;
char tmp[100];
FILE *fp;
int i, j, result, find_flag;
@@ -401,7 +394,7 @@
*/
inv_error_t inv_get_iio_trigger_path(const char *name)
{
- if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0)
+ if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0)
return INV_ERROR_NOT_OPENED;
else
return INV_SUCCESS;
@@ -416,7 +409,7 @@
*/
inv_error_t inv_get_iio_device_node(const char *name)
{
- if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0)
+ if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0)
return INV_ERROR_NOT_OPENED;
else
return INV_SUCCESS;
diff --git a/libsensors_iio/software/core/mllite/linux/mlos.h b/libsensors_iio/software/core/mllite/linux/mlos.h
index d4f8912..287025f 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos.h
+++ b/libsensors_iio/software/core/mllite/linux/mlos.h
@@ -10,7 +10,6 @@
#ifndef __KERNEL__
#include <stdio.h>
#endif
-#include <pthread.h>
#include "mltypes.h"
@@ -19,7 +18,7 @@
#endif
#if defined(LINUX) || defined(__KERNEL__)
-typedef pthread_mutex_t* HANDLE;
+typedef unsigned int 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 5424508..75f062e 100644
--- a/libsensors_iio/software/core/mllite/linux/mlos_linux.c
+++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c
@@ -1,14 +1,13 @@
/*
$License:
- Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
$
*/
-
/*******************************************************************************
*
* $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
*
- ******************************************************************************/
+ *******************************************************************************/
/**
* @defgroup MLOS
@@ -17,7 +16,7 @@
* @{
* @file mlos.c
* @brief OS Interface.
- */
+**/
/* ------------- */
/* - Includes. - */
@@ -27,10 +26,11 @@
#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 num_bytes number of bytes
+ * @param numBytes number of bytes
* @return pointer to allocated space
- */
-void *inv_malloc(unsigned int num_bytes)
+**/
+void *inv_malloc(unsigned int numBytes)
{
// Allocate space.
- void *alloc_ptr = malloc(num_bytes);
- return alloc_ptr;
+ void *allocPtr = malloc(numBytes);
+ return allocPtr;
}
@@ -54,11 +54,12 @@
* @brief Free allocated space
* @param ptr pointer to space to deallocate
* @return error code.
- */
+**/
inv_error_t inv_free(void *ptr)
{
- if (ptr)
- free(ptr);
+ // Deallocate space.
+ free(ptr);
+
return INV_SUCCESS;
}
@@ -95,10 +96,10 @@
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;
@@ -109,11 +110,11 @@
* @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)
@@ -130,7 +131,7 @@
*/
FILE *inv_fopen(char *filename)
{
- FILE *fp = fopen(filename, "r");
+ FILE *fp = fopen(filename,"r");
return fp;
}
@@ -153,21 +154,22 @@
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 m_secs)
+void inv_sleep(int mSecs)
{
- usleep(m_secs * 1000);
+ usleep(mSecs*1000);
}
@@ -180,11 +182,13 @@
{
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/message_layer.c b/libsensors_iio/software/core/mllite/message_layer.c
index d266d80..8317957 100644
--- a/libsensors_iio/software/core/mllite/message_layer.c
+++ b/libsensors_iio/software/core/mllite/message_layer.c
@@ -1,59 +1,59 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup Message_Layer message_layer
- * @brief Motion Library - Message Layer
- * Holds Low Occurance messages
- *
- * @{
- * @file message_layer.c
- * @brief Holds Low Occurance Messages.
- */
-#include "message_layer.h"
-#include "log.h"
-
-struct message_holder_t {
- long message;
-};
-
-static struct message_holder_t mh;
-
-/** Sets a message.
-* @param[in] set The flags to set.
-* @param[in] clear Before setting anything this will clear these messages,
-* which is useful for mutually exclusive messages such
-* a motion or no motion message.
-* @param[in] level Level of the messages. It starts at 0, and may increase
-* in the future to allow more messages if the bit storage runs out.
-*/
-void inv_set_message(long set, long clear, int level)
-{
- if (level == 0) {
- mh.message &= ~clear;
- mh.message |= set;
- }
-}
-
-/** Returns Message Flags for Level 0 Messages.
-* Levels are to allow expansion of more messages in the future.
-* @param[in] clear If set, will clear the message. Typically this will be set
-* for one reader, so that you don't get the same message over and over.
-* @return bit field to corresponding message.
-*/
-long inv_get_message_level_0(int clear)
-{
- long msg;
- msg = mh.message;
- if (clear) {
- mh.message = 0;
- }
- return msg;
-}
-
-/**
- * @}
- */
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+/**
+ * @defgroup Message_Layer message_layer
+ * @brief Motion Library - Message Layer
+ * Holds Low Occurance messages
+ *
+ * @{
+ * @file message_layer.c
+ * @brief Holds Low Occurance Messages.
+ */
+#include "message_layer.h"
+#include "log.h"
+
+struct message_holder_t {
+ long message;
+};
+
+static struct message_holder_t mh;
+
+/** Sets a message.
+* @param[in] set The flags to set.
+* @param[in] clear Before setting anything this will clear these messages,
+* which is useful for mutually exclusive messages such
+* a motion or no motion message.
+* @param[in] level Level of the messages. It starts at 0, and may increase
+* in the future to allow more messages if the bit storage runs out.
+*/
+void inv_set_message(long set, long clear, int level)
+{
+ if (level == 0) {
+ mh.message &= ~clear;
+ mh.message |= set;
+ }
+}
+
+/** Returns Message Flags for Level 0 Messages.
+* Levels are to allow expansion of more messages in the future.
+* @param[in] clear If set, will clear the message. Typically this will be set
+* for one reader, so that you don't get the same message over and over.
+* @return bit field to corresponding message.
+*/
+long inv_get_message_level_0(int clear)
+{
+ long msg;
+ msg = mh.message;
+ if (clear) {
+ mh.message = 0;
+ }
+ return msg;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/message_layer.h b/libsensors_iio/software/core/mllite/message_layer.h
index e6d90d5..df0c0e2 100644
--- a/libsensors_iio/software/core/mllite/message_layer.h
+++ b/libsensors_iio/software/core/mllite/message_layer.h
@@ -1,35 +1,35 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_MESSAGE_LAYER_H__
-#define INV_MESSAGE_LAYER_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* Level 0 Type Messages */
- /** A motion event has occured */
-#define INV_MSG_MOTION_EVENT (0x01)
- /** A no motion event has occured */
-#define INV_MSG_NO_MOTION_EVENT (0x02)
- /** A setting of the gyro bias has occured */
-#define INV_MSG_NEW_GB_EVENT (0x04)
- /** A setting of the compass bias has occured */
-#define INV_MSG_NEW_CB_EVENT (0x08)
- /** A setting of the accel bias has occured */
-#define INV_MSG_NEW_AB_EVENT (0x10)
-
- void inv_set_message(long set, long clear, int level);
- long inv_get_message_level_0(int clear);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MESSAGE_LAYER_H__
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_MESSAGE_LAYER_H__
+#define INV_MESSAGE_LAYER_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /* Level 0 Type Messages */
+ /** A motion event has occured */
+#define INV_MSG_MOTION_EVENT (0x01)
+ /** A no motion event has occured */
+#define INV_MSG_NO_MOTION_EVENT (0x02)
+ /** A setting of the gyro bias has occured */
+#define INV_MSG_NEW_GB_EVENT (0x04)
+ /** A setting of the compass bias has occured */
+#define INV_MSG_NEW_CB_EVENT (0x08)
+ /** A setting of the accel bias has occured */
+#define INV_MSG_NEW_AB_EVENT (0x10)
+
+ void inv_set_message(long set, long clear, int level);
+ long inv_get_message_level_0(int clear);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MESSAGE_LAYER_H__
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c
index cce0381..86c4b41 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.c
+++ b/libsensors_iio/software/core/mllite/ml_math_func.c
@@ -655,52 +655,6 @@
{
return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
}
-
-void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
- int i;
- // initial state to zero
- pFilter->state[0] = 0;
- pFilter->state[1] = 0;
-
- // set up coefficients
- for (i=0; i<5; i++) {
- pFilter->c[i] = pBiquadCoeff[i];
- }
-}
-
-void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
-{
- pFilter->input = input;
- pFilter->output = input;
- pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
- pFilter->state[1] = pFilter->state[0];
-}
-
-float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) {
- float stateZero;
-
- pFilter->input = input;
- // calculate the new state;
- stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0]
- - pFilter->c[3]*pFilter->state[1];
-
- pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0]
- + pFilter->c[1]*pFilter->state[1];
-
- // update the output and state
- pFilter->output = pFilter->output * pFilter->c[4];
- pFilter->state[1] = pFilter->state[0];
- pFilter->state[0] = stateZero;
- return pFilter->output;
-}
-
-void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) {
-
- cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
- cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
- cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
-}
-
/**
* @}
*/
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.h b/libsensors_iio/software/core/mllite/ml_math_func.h
index 535d849..916de0a 100644
--- a/libsensors_iio/software/core/mllite/ml_math_func.h
+++ b/libsensors_iio/software/core/mllite/ml_math_func.h
@@ -24,13 +24,6 @@
extern "C" {
#endif
- typedef struct {
- float state[4];
- float c[5];
- float input;
- float output;
- } inv_biquad_filter_t;
-
static inline float inv_q30_to_float(long q30)
{
return (float) q30 / ((float)(1L << 30));
@@ -109,11 +102,6 @@
double quaternion_to_rotation_angle(const long *quat);
double inv_vector_norm(const float *x);
- void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff);
- float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input);
- void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input);
- void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]);
-
#ifdef __cplusplus
}
#endif
diff --git a/libsensors_iio/software/core/mllite/mlmath.c b/libsensors_iio/software/core/mllite/mlmath.c
new file mode 100644
index 0000000..5eb4264
--- /dev/null
+++ b/libsensors_iio/software/core/mllite/mlmath.c
@@ -0,0 +1,68 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *******************************************************************************/
+
+#include <math.h>
+
+double ml_asin(double x)
+{
+ return asin(x);
+}
+
+double ml_atan(double x)
+{
+ return atan(x);
+}
+
+double ml_atan2(double x, double y)
+{
+ return atan2(x, y);
+}
+
+double ml_log(double x)
+{
+ return log(x);
+}
+
+double ml_sqrt(double x)
+{
+ return sqrt(x);
+}
+
+double ml_ceil(double x)
+{
+ return ceil(x);
+}
+
+double ml_floor(double x)
+{
+ return floor(x);
+}
+
+double ml_cos(double x)
+{
+ return cos(x);
+}
+
+double ml_sin(double x)
+{
+ return sin(x);
+}
+
+double ml_acos(double x)
+{
+ return acos(x);
+}
+
+double ml_pow(double x, double y)
+{
+ return pow(x, y);
+}
diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c
index ec87fc0..231cbfd 100644
--- a/libsensors_iio/software/core/mllite/mpl.c
+++ b/libsensors_iio/software/core/mllite/mpl.c
@@ -1,72 +1,72 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup MPL mpl
- * @brief Motion Library - Start Point
- * Initializes MPL.
- *
- * @{
- * @file mpl.c
- * @brief MPL start point.
- */
-
-#include "storage_manager.h"
-#include "log.h"
-#include "mpl.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "results_holder.h"
-#include "mlinclude.h"
-
-/**
- * @brief Initializes the MPL. Should be called first and once
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_init_mpl(void)
-{
- inv_init_storage_manager();
-
- /* initialize the start callback manager */
- INV_ERROR_CHECK(inv_init_start_manager());
-
- /* initialize the data builder */
- INV_ERROR_CHECK(inv_init_data_builder());
-
- INV_ERROR_CHECK(inv_enable_results_holder());
-
- return INV_SUCCESS;
-}
-
-const char ml_ver[] = "InvenSense MPL 5.1.2";
-
-/**
- * @brief used to get the MPL version.
- * @param version a string where the MPL version gets stored.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_get_version(char **version)
-{
- INVENSENSE_FUNC_START;
- /* cast out the const */
- *version = (char *)&ml_ver;
- return INV_SUCCESS;
-}
-
-/**
- * @brief Starts the MPL. Typically called after inv_init_mpl() or after a
- * inv_stop_mpl() to start the MPL back up an running.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_start_mpl(void)
-{
- INV_ERROR_CHECK(inv_execute_mpl_start_notification());
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+/**
+ * @defgroup MPL mpl
+ * @brief Motion Library - Start Point
+ * Initializes MPL.
+ *
+ * @{
+ * @file mpl.c
+ * @brief MPL start point.
+ */
+
+#include "storage_manager.h"
+#include "log.h"
+#include "mpl.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+#include "mlinclude.h"
+
+/**
+ * @brief Initializes the MPL. Should be called first and once
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_init_mpl(void)
+{
+ inv_init_storage_manager();
+
+ /* initialize the start callback manager */
+ INV_ERROR_CHECK(inv_init_start_manager());
+
+ /* initialize the data builder */
+ INV_ERROR_CHECK(inv_init_data_builder());
+
+ INV_ERROR_CHECK(inv_enable_results_holder());
+
+ return INV_SUCCESS;
+}
+
+const char ml_ver[] = "InvenSense MPL 5.0.1";
+
+/**
+ * @brief used to get the MPL version.
+ * @param version a string where the MPL version gets stored.
+ * @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_get_version(char **version)
+{
+ INVENSENSE_FUNC_START;
+ /* cast out the const */
+ *version = (char *)&ml_ver;
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief Starts the MPL. Typically called after inv_init_mpl() or after a
+ * inv_stop_mpl() to start the MPL back up an running.
+ * @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_start_mpl(void)
+{
+ INV_ERROR_CHECK(inv_execute_mpl_start_notification());
+ return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c
index df58f40..97ffdec 100644
--- a/libsensors_iio/software/core/mllite/results_holder.c
+++ b/libsensors_iio/software/core/mllite/results_holder.c
@@ -13,16 +13,13 @@
* @file results_holder.c
* @brief Results Holder for HAL.
*/
-
-#include <string.h>
-
#include "results_holder.h"
+#include "log.h"
#include "ml_math_func.h"
#include "mlmath.h"
#include "start_manager.h"
#include "data_builder.h"
#include "message_layer.h"
-#include "log.h"
// These 2 status bits are used to control when the 9 axis quaternion is updated
#define INV_COMPASS_CORRECTION_SET 1
@@ -37,7 +34,6 @@
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 */
@@ -322,24 +318,6 @@
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 09da24e..a60d7f0 100644
--- a/libsensors_iio/software/core/mllite/results_holder.h
+++ b/libsensors_iio/software/core/mllite/results_holder.h
@@ -70,10 +70,6 @@
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/start_manager.c b/libsensors_iio/software/core/mllite/start_manager.c
index 3e082d4..fb758e7 100644
--- a/libsensors_iio/software/core/mllite/start_manager.c
+++ b/libsensors_iio/software/core/mllite/start_manager.c
@@ -1,105 +1,105 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-/**
- * @defgroup Start_Manager start_manager
- * @brief Motion Library - Start Manager
- * Start Manager
- *
- * @{
- * @file start_manager.c
- * @brief This handles all the callbacks when inv_start_mpl() is called.
- */
-
-
-#include <string.h>
-#include "log.h"
-#include "start_manager.h"
-
-typedef inv_error_t (*inv_start_cb_func)();
-struct inv_start_cb_t {
- int num_cb;
- inv_start_cb_func start_cb[INV_MAX_START_CB];
-};
-
-static struct inv_start_cb_t inv_start_cb;
-
-/** Initilize the start manager. Typically called by inv_start_mpl();
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_start_manager(void)
-{
- memset(&inv_start_cb, 0, sizeof(inv_start_cb));
- return INV_SUCCESS;
-}
-
-/** Removes a callback from start notification
-* @param[in] start_cb function to remove from start notification
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- int kk;
-
- for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
- if (inv_start_cb.start_cb[kk] == start_cb) {
- // Found the match
- if (kk != (inv_start_cb.num_cb-1)) {
- memmove(&inv_start_cb.start_cb[kk],
- &inv_start_cb.start_cb[kk+1],
- (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
- }
- inv_start_cb.num_cb--;
- return INV_SUCCESS;
- }
- }
- return INV_ERROR_INVALID_PARAMETER;
-}
-
-/** Register a callback to receive when inv_start_mpl() is called.
-* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- if (inv_start_cb.num_cb >= INV_MAX_START_CB)
- return INV_ERROR_INVALID_PARAMETER;
-
- inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
- inv_start_cb.num_cb++;
- return INV_SUCCESS;
-}
-
-/** Callback all the functions that want to be notified when inv_start_mpl() was
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_execute_mpl_start_notification(void)
-{
- inv_error_t result,first_error;
- int kk;
-
- first_error = INV_SUCCESS;
-
- for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
- result = inv_start_cb.start_cb[kk]();
- if (result && (first_error == INV_SUCCESS)) {
- first_error = result;
- }
- }
- return first_error;
-}
-
-/**
- * @}
- */
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+/**
+ * @defgroup Start_Manager start_manager
+ * @brief Motion Library - Start Manager
+ * Start Manager
+ *
+ * @{
+ * @file start_manager.c
+ * @brief This handles all the callbacks when inv_start_mpl() is called.
+ */
+
+
+#include <string.h>
+#include "log.h"
+#include "start_manager.h"
+
+typedef inv_error_t (*inv_start_cb_func)();
+struct inv_start_cb_t {
+ int num_cb;
+ inv_start_cb_func start_cb[INV_MAX_START_CB];
+};
+
+static struct inv_start_cb_t inv_start_cb;
+
+/** Initilize the start manager. Typically called by inv_start_mpl();
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_start_manager(void)
+{
+ memset(&inv_start_cb, 0, sizeof(inv_start_cb));
+ return INV_SUCCESS;
+}
+
+/** Removes a callback from start notification
+* @param[in] start_cb function to remove from start notification
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
+{
+ int kk;
+
+ for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
+ if (inv_start_cb.start_cb[kk] == start_cb) {
+ // Found the match
+ if (kk != (inv_start_cb.num_cb-1)) {
+ memmove(&inv_start_cb.start_cb[kk],
+ &inv_start_cb.start_cb[kk+1],
+ (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
+ }
+ inv_start_cb.num_cb--;
+ return INV_SUCCESS;
+ }
+ }
+ return INV_ERROR_INVALID_PARAMETER;
+}
+
+/** Register a callback to receive when inv_start_mpl() is called.
+* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
+* called.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
+{
+ if (inv_start_cb.num_cb >= INV_MAX_START_CB)
+ return INV_ERROR_INVALID_PARAMETER;
+
+ inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
+ inv_start_cb.num_cb++;
+ return INV_SUCCESS;
+}
+
+/** Callback all the functions that want to be notified when inv_start_mpl() was
+* called.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_execute_mpl_start_notification(void)
+{
+ inv_error_t result,first_error;
+ int kk;
+
+ first_error = INV_SUCCESS;
+
+ for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
+ result = inv_start_cb.start_cb[kk]();
+ if (result && (first_error == INV_SUCCESS)) {
+ first_error = result;
+ }
+ }
+ return first_error;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c
index 3e4e619..4b92bfc 100644
--- a/libsensors_iio/software/core/mllite/storage_manager.c
+++ b/libsensors_iio/software/core/mllite/storage_manager.c
@@ -4,7 +4,6 @@
See included License.txt for License information.
$
*/
-
/**
* @defgroup Storage_Manager storage_manager
* @brief Motion Library - Stores Data for functions.
@@ -14,9 +13,6 @@
* @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/accel_auto_cal.h b/libsensors_iio/software/core/mpl/accel_auto_cal.h
index 6641a7f..5a53213 100644
--- a/libsensors_iio/software/core/mpl/accel_auto_cal.h
+++ b/libsensors_iio/software/core/mpl/accel_auto_cal.h
@@ -1,38 +1,38 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id$
- *
- ******************************************************************************/
-
-#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__
-#define MLDMP_ACCEL_AUTO_CALIBRATION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_in_use_auto_calibration(void);
-inv_error_t inv_disable_in_use_auto_calibration(void);
-inv_error_t inv_stop_in_use_auto_calibration(void);
-inv_error_t inv_start_in_use_auto_calibration(void);
-inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
-inv_error_t inv_init_in_use_auto_calibration(void);
-void inv_init_accel_maxmin(void);
-void inv_record_good_accel_maxmin(void);
-int inv_get_accel_bias_stage();
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__
-
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id$
+ *
+ ******************************************************************************/
+
+#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__
+#define MLDMP_ACCEL_AUTO_CALIBRATION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_in_use_auto_calibration(void);
+inv_error_t inv_disable_in_use_auto_calibration(void);
+inv_error_t inv_stop_in_use_auto_calibration(void);
+inv_error_t inv_start_in_use_auto_calibration(void);
+inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
+inv_error_t inv_init_in_use_auto_calibration(void);
+void inv_init_accel_maxmin(void);
+void inv_record_good_accel_maxmin(void);
+int inv_get_accel_bias_stage();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__
+
diff --git a/libsensors_iio/software/core/mpl/adv_func.h b/libsensors_iio/software/core/mpl/adv_func.h
new file mode 100644
index 0000000..3f8595f
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/adv_func.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_ADVFUNC_H__
+#define MLDMP_ADVFUNC_H__
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ float inv_compass_angle(const long *compass, const long *grav, const float *quat);
+ inv_error_t inv_set_dmp_quaternion(long *q);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_ADVFUNC_H__
diff --git a/libsensors_iio/software/core/mpl/auto_calibration.h b/libsensors_iio/software/core/mpl/auto_calibration.h
new file mode 100644
index 0000000..3dd9827
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/auto_calibration.h
@@ -0,0 +1,33 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_IN_USE_AUTO_CALIBRATION_H__
+#define MLDMP_IN_USE_AUTO_CALIBRATION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_in_use_auto_calibration(void);
+inv_error_t inv_disable_in_use_auto_calibration(void);
+inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
+inv_error_t inv_init_in_use_auto_calibration(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_IN_USE_AUTO_CALIBRATION_H__
+
diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
index 5e74a60..116b618 100644
--- a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
+++ b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
Binary files differ
diff --git a/libsensors_iio/software/core/mpl/build/android/shared.mk b/libsensors_iio/software/core/mpl/build/android/shared.mk
index 2e15205..79cf9c1 100644
--- a/libsensors_iio/software/core/mpl/build/android/shared.mk
+++ b/libsensors_iio/software/core/mpl/build/android/shared.mk
@@ -16,7 +16,6 @@
include $(INV_ROOT)/software/build/android/common.mk
CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += $(ANDROID_COMPILE)
CFLAGS += -Wall
CFLAGS += -fpic
CFLAGS += -nostdlib
@@ -44,6 +43,11 @@
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/build/android/static.mk b/libsensors_iio/software/core/mpl/build/android/static.mk
new file mode 100644
index 0000000..6ad45de
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/build/android/static.mk
@@ -0,0 +1,110 @@
+MLLITE_LIB_NAME = mllite
+LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT)
+
+MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
+
+OBJFOLDER = $(CURDIR)/obj
+
+CROSS = arm-none-linux-gnueabi-
+COMP= $(CROSS)gcc
+LINK= $(CROSS)ar cr
+
+MLLITE_DIR = $(MLSDK_ROOT)/mllite
+MPL_DIR = $(MLSDK_ROOT)/mldmp
+MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
+
+include $(MLSDK_ROOT)/Android-common.mk
+
+CFLAGS += $(CMDLINE_CFLAGS)
+CFLAGS += -Wall -fpic
+CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0
+CFLAGS += -DNDEBUG
+CFLAGS += -D_REENTRANT -DLINUX -DANDROID
+CFLAGS += -I$(MLLITE_DIR)
+CFLAGS += -I$(MLPLATFORM_DIR)/include
+CFLAGS += -I$(MLSDK_ROOT)/mlutils
+CFLAGS += -I$(MLSDK_ROOT)/mlapps/common
+CFLAGS += $(MLSDK_INCLUDES)
+CFLAGS += $(MLSDK_DEFINES)
+
+VPATH += $(MLLITE_DIR)
+VPATH += $(MLSDK_ROOT)/mlutils
+VPATH += $(MLLITE_DIR)/accel
+VPATH += $(MLLITE_DIR)/compass
+VPATH += $(MLLITE_DIR)/pressure
+VPATH += $(MLLITE_DIR)/mlapps/common
+
+####################################################################################################
+## sources
+
+ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT)
+
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c
+ML_SOURCES += $(MLLITE_DIR)/accel.c
+ML_SOURCES += $(MLLITE_DIR)/compass.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c
+ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c
+ML_SOURCES += $(MLLITE_DIR)/key0_96.c
+ML_SOURCES += $(MLLITE_DIR)/pressure.c
+ML_SOURCES += $(MLLITE_DIR)/ml.c
+ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c
+ML_SOURCES += $(MLLITE_DIR)/ml_init.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c
+ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c
+ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c
+ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c
+ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c
+ML_SOURCES += $(MLLITE_DIR)/mldl.c
+ML_SOURCES += $(MLLITE_DIR)/mldmp.c
+ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c
+ML_SOURCES += $(MLLITE_DIR)/mlstates.c
+ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c
+ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c
+ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c
+ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c
+ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c
+ifeq ($(MPU_NAME),MPU3050)
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c
+else
+ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c
+endif
+
+ML_OBJS := $(addsuffix .o,$(ML_SOURCES))
+ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES))))
+
+####################################################################################################
+## rules
+
+.PHONY: all clean cleanall
+
+all: $(LIBRARY) $(MK_NAME)
+
+$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME)
+ @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n")
+ $(LINK) $(LIBRARY) $(ML_OBJS_DST)
+ $(CROSS)ranlib $(LIBRARY)
+
+$(OBJFOLDER) :
+ @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
+ mkdir obj
+
+$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME)
+ @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
+ $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $<
+
+clean :
+ rm -fR $(OBJFOLDER)
+
+cleanall :
+ rm -fR $(LIBRARY) $(OBJFOLDER)
+
diff --git a/libsensors_iio/software/core/mpl/fast_no_motion.h b/libsensors_iio/software/core/mpl/fast_no_motion.h
index 24bc0d5..2a33093 100644
--- a/libsensors_iio/software/core/mpl/fast_no_motion.h
+++ b/libsensors_iio/software/core/mpl/fast_no_motion.h
@@ -28,14 +28,12 @@
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);
+
+ 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
}
diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/libsensors_iio/software/core/mpl/fusion_9axis.h
index 1ba1ebb..616694a 100644
--- a/libsensors_iio/software/core/mpl/fusion_9axis.h
+++ b/libsensors_iio/software/core/mpl/fusion_9axis.h
@@ -26,8 +26,6 @@
inv_error_t inv_disable_9x_sensor_fusion(void);
inv_error_t inv_start_9x_sensor_fusion(void);
inv_error_t inv_stop_9x_sensor_fusion(void);
- inv_error_t inv_9x_fusion_set_mag_fb(double fb);
- inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);
#ifdef __cplusplus
}
diff --git a/libsensors_iio/software/core/mpl/interpolator.h b/libsensors_iio/software/core/mpl/interpolator.h
new file mode 100644
index 0000000..5eb571d
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/interpolator.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef INTERPOLATOR_H
+#define INTERPOLATOR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//#include "mltypes.h"
+
+ /* ------------ */
+ /* - Defines. - */
+ /* ------------ */
+
+#define MAX_INTERPOLATION (20)
+
+typedef struct {
+ long x[2];
+ long y[4];
+} tInterpolate2;
+typedef struct {
+ long x[2];
+ long y[6];
+} tInterpolate3;
+typedef struct {
+ long x[5];
+ long y[10];
+ int idx1;
+} tInterpolate5;
+typedef struct {
+ tInterpolate2 state1;
+ tInterpolate2 state2;
+} tInterpolate4;
+typedef struct {
+ tInterpolate3 state1;
+ tInterpolate2 state2;
+} tInterpolate6;
+typedef struct {
+ tInterpolate2 state1;
+ tInterpolate4 state2;
+} tInterpolate8;
+typedef struct {
+ tInterpolate3 state1;
+ tInterpolate3 state2;
+} tInterpolate9;
+typedef struct {
+ tInterpolate5 state1;
+ tInterpolate2 state2;
+} tInterpolate10;
+typedef struct {
+ tInterpolate4 state1;
+ tInterpolate3 state2;
+} tInterpolate12;
+typedef struct {
+ tInterpolate5 state1;
+ tInterpolate3 state2;
+} tInterpolate15;
+typedef struct {
+ tInterpolate2 state1;
+ tInterpolate8 state2;
+} tInterpolate16;
+typedef struct {
+ tInterpolate9 state1;
+ tInterpolate2 state2;
+} tInterpolate18;
+typedef struct {
+ tInterpolate10 state1;
+ tInterpolate2 state2;
+} tInterpolate20;
+
+typedef union {
+ tInterpolate2 u2;
+ tInterpolate3 u3;
+ tInterpolate4 u4;
+ tInterpolate5 u5;
+ tInterpolate6 u6;
+ tInterpolate8 u8;
+ tInterpolate9 u9;
+ tInterpolate10 u10;
+ tInterpolate12 u12;
+ tInterpolate15 u15;
+ tInterpolate16 u16;
+ tInterpolate18 u18;
+ tInterpolate20 u20;
+} tInterpolateState;
+
+ /* --------------------- */
+ /* - Function p-types. - */
+ /* --------------------- */
+int inv_get_interp_amount( int x );
+int inv_interpolate( int amount, long input, long *output, tInterpolateState *state );
+long inv_fxmult( long x, long y );
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* INTERPOLATOR_H */
diff --git a/libsensors_iio/software/core/mpl/inv_log.h b/libsensors_iio/software/core/mpl/inv_log.h
new file mode 100644
index 0000000..972844b
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/inv_log.h
@@ -0,0 +1,7 @@
+#include "mltypes.h"
+#ifndef INV_INV_LOG_H__
+#define INV_INV_LOG_H__
+
+#define INV_LOGE(s)
+
+#endif // INV_INV_LOG_H__
diff --git a/libsensors_iio/software/core/mpl/inv_math.h b/libsensors_iio/software/core/mpl/inv_math.h
index 175511a..6620bbf 100644
--- a/libsensors_iio/software/core/mpl/inv_math.h
+++ b/libsensors_iio/software/core/mpl/inv_math.h
@@ -1,8 +1,8 @@
-/* math.h has many functions and defines that are not consistent across
-* platforms. This address that */
-
-#ifdef _WINDOWS
-#define _USE_MATH_DEFINES
-#endif
-
-#include <math.h>
+/* math.h has many functions and defines that are not consistent across
+* platforms. This address that */
+
+#ifdef _WINDOWS
+#define _USE_MATH_DEFINES
+#endif
+
+#include <math.h>
diff --git a/libsensors_iio/software/core/mpl/invensense_adv.h b/libsensors_iio/software/core/mpl/invensense_adv.h
index 12932c9..9e59c18 100644
--- a/libsensors_iio/software/core/mpl/invensense_adv.h
+++ b/libsensors_iio/software/core/mpl/invensense_adv.h
@@ -28,4 +28,3 @@
#include "quaternion_supervisor.h"
#include "mag_disturb.h"
#include "quat_accuracy_monitor.h"
-#include "shake.h"
diff --git a/libsensors_iio/software/core/mpl/mlsetinterrupts.h b/libsensors_iio/software/core/mpl/mlsetinterrupts.h
new file mode 100644
index 0000000..a81dabb
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/mlsetinterrupts.h
@@ -0,0 +1,23 @@
+/*
+ $License:
+ Copyright (c) 2008 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+#ifndef MLSETINTERRUPT_H
+#define MLSETINTERRUPT_H
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /* DEPRECATED - Scheduled for removal. Do not use */
+ inv_error_t MLSetInterrupts(unsigned short interrupts);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* MLSETINTERRUPT_H */
diff --git a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h
new file mode 100644
index 0000000..3779381
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h
@@ -0,0 +1,57 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlsupervisor_9axis.h 6123 2011-09-30 18:21:11Z mcaramello $
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_MLSUPERVISOR_H__
+#define MLDMP_MLSUPERVISOR_H__
+
+#include "mltypes.h"
+
+//#include "temp_comp.h"
+
+struct inv_fusion_t {
+ int compassCount;
+ long quat[4];
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_9x_fusion(void);
+inv_error_t inv_disable_9x_fusion(void);
+
+inv_error_t inv_enable_9x_fusion_legacy(void);
+inv_error_t inv_disable_9x_fusion_legacy(void);
+
+inv_error_t inv_enable_9x_fusion_new(void);
+inv_error_t inv_disable_9x_fusion_new(void);
+
+inv_error_t inv_enable_9x_fusion_basic(void);
+inv_error_t inv_disable_9x_fusion_basic(void);
+
+inv_error_t inv_enable_9x_fusion_external(void);
+inv_error_t inv_disable_9x_fusion_external(void);
+
+inv_error_t inv_enable_maintain_heading(void);
+inv_error_t inv_disable_maintain_heading(void);
+
+void inv_set_compass_state(long compassState, long accState,
+ unsigned long deltaTime,
+ int magDisturb, int gotBias,
+ int *new_state,
+ int *new_accuracy);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_MLSUPERVISOR_H__
diff --git a/libsensors_iio/software/core/mpl/motion_no_motion.h b/libsensors_iio/software/core/mpl/motion_no_motion.h
index 188c78b..01cf1c0 100644
--- a/libsensors_iio/software/core/mpl/motion_no_motion.h
+++ b/libsensors_iio/software/core/mpl/motion_no_motion.h
@@ -1,28 +1,28 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
*/
-#ifndef INV_MOTION_NO_MOTION_H__
-#define INV_MOTION_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_motion_no_motion(void);
-inv_error_t inv_disable_motion_no_motion(void);
-inv_error_t inv_init_motion_no_motion(void);
-inv_error_t inv_start_motion_no_motion(void);
-inv_error_t inv_stop_motion_no_motion(void);
-
-inv_error_t inv_set_no_motion_time(long time_ms);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MOTION_NO_MOTION_H__
+#ifndef INV_MOTION_NO_MOTION_H__
+#define INV_MOTION_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_motion_no_motion(void);
+inv_error_t inv_disable_motion_no_motion(void);
+inv_error_t inv_init_motion_no_motion(void);
+inv_error_t inv_start_motion_no_motion(void);
+inv_error_t inv_stop_motion_no_motion(void);
+
+inv_error_t inv_set_no_motion_time(long time_ms);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MOTION_NO_MOTION_H__
diff --git a/libsensors_iio/software/core/mpl/orientation.h b/libsensors_iio/software/core/mpl/orientation.h
new file mode 100644
index 0000000..ab4e45e
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/orientation.h
@@ -0,0 +1,42 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef MLDMP_ORIENTATION_H__
+#define MLDMP_ORIENTATION_H__
+
+#include "mltypes.h"
+/*******************************************************************************/
+/* Orientations */
+/*******************************************************************************/
+
+#define INV_X_UP 0x01
+#define INV_X_DOWN 0x02
+#define INV_Y_UP 0x04
+#define INV_Y_DOWN 0x08
+#define INV_Z_UP 0x10
+#define INV_Z_DOWN 0x20
+#define INV_ORIENTATION_ALL 0x3F
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ inv_error_t inv_enable_orientation(void);
+ inv_error_t inv_disable_orientation(void);
+ inv_error_t inv_set_orientation(int orientation);
+ inv_error_t inv_set_orientation_cb(void (*callback)(unsigned short));
+ inv_error_t inv_get_orientation(int *orientation);
+ inv_error_t inv_get_orientation_state(int * state);
+ inv_error_t inv_set_orientation_interrupt(unsigned char on);
+ inv_error_t inv_set_orientation_thresh(float angle,
+ float hysteresis,
+ unsigned long time,
+ unsigned int axis);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_ORIENTATION_H__
diff --git a/libsensors_iio/software/core/mpl/progressive_no_motion.h b/libsensors_iio/software/core/mpl/progressive_no_motion.h
new file mode 100644
index 0000000..99333e3
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/progressive_no_motion.h
@@ -0,0 +1,39 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id:$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_PROG_NO_MOTION_H__
+#define MLDMP_PROG_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#define PROG_NO_MOTION 1
+#define PROG_MOTION 2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* APIs */
+inv_error_t inv_enable_prog_no_motion(void);
+inv_error_t inv_disable_prog_no_motion(void);
+
+/* internal use */
+int inv_get_prog_no_motion_enabled(void);
+void inv_get_prog_no_motion_bias_changed(void);
+int inv_get_prog_no_motion_state(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_PROG_NO_MOTION_H__
diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
index 5ee0573..2cf7a50 100644
--- a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
+++ b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
@@ -1,71 +1,70 @@
-/*
- quat_accuracy_monitor.h
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-#ifndef QUAT_ACCURARCY_MONITOR_H__
-#define QUAT_ACCURARCY_MONITOR_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-enum accuracy_signal_type_e {
- TYPE_NAV_QUAT,
- TYPE_GAM_QUAT,
- TYPE_NAV_QUAT_ADVANCED,
- TYPE_GAM_QUAT_ADVANCED,
- TYPE_NAV_QUAT_BASIC,
- TYPE_GAM_QUAT_BASIC,
- TYPE_MAG,
- TYPE_GYRO,
- TYPE_ACCEL,
-};
-
-inv_error_t inv_init_quat_accuracy_monitor(void);
-
-void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
-double get_accuracy_threshold(enum accuracy_signal_type_e type);
-void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
-int get_accuracy_weight(enum accuracy_signal_type_e type);
-
-int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
-
-void inv_reset_quat_accuracy(void);
-double get_6axis_correction_term(void);
-double get_9axis_correction_term(void);
-int get_9axis_accuracy_state();
-
-void set_6axis_error_average(double value);
-double get_6axis_error_bound(void);
-double get_compass_correction(void);
-double get_9axis_error_bound(void);
-
-float get_confidence_interval(void);
-void set_compass_uncertainty(float value);
-
-inv_error_t inv_enable_quat_accuracy_monitor(void);
-inv_error_t inv_disable_quat_accuracy_monitor(void);
-inv_error_t inv_start_quat_accuracy_monitor(void);
-inv_error_t inv_stop_quat_accuracy_monitor(void);
-
-double get_compassNgravity(void);
-double get_init_compassNgravity(void);
-
-float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // QUAT_ACCURARCY_MONITOR_H__
+/*
+ quat_accuracy_monitor.h
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef QUAT_ACCURARCY_MONITOR_H__
+#define QUAT_ACCURARCY_MONITOR_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+enum accuracy_signal_type_e {
+ TYPE_NAV_QUAT,
+ TYPE_GAM_QUAT,
+ TYPE_NAV_QUAT_ADVANCED,
+ TYPE_GAM_QUAT_ADVANCED,
+ TYPE_NAV_QUAT_BASIC,
+ TYPE_GAM_QUAT_BASIC,
+ TYPE_MAG,
+ TYPE_GYRO,
+ TYPE_ACCEL,
+};
+
+inv_error_t inv_init_quat_accuracy_monitor(void);
+
+void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
+double get_accuracy_threshold(enum accuracy_signal_type_e type);
+void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
+int get_accuracy_weight(enum accuracy_signal_type_e type);
+
+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
+
+void inv_reset_quat_accuracy(void);
+double get_6axis_correction_term(void);
+double get_9axis_correction_term(void);
+int get_9axis_accuracy_state();
+
+void set_6axis_error_average(double value);
+double get_6axis_error_bound(void);
+double get_compass_correction(void);
+double get_9axis_error_bound(void);
+
+float get_confidence_interval(void);
+void set_compass_uncertainty(float value);
+
+inv_error_t inv_enable_quat_accuracy_monitor(void);
+inv_error_t inv_disable_quat_accuracy_monitor(void);
+inv_error_t inv_start_quat_accuracy_monitor(void);
+inv_error_t inv_stop_quat_accuracy_monitor(void);
+
+double get_compassNgravity(void);
+double get_init_compassNgravity(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
index 722c0d9..532e8af 100644
--- a/libsensors_iio/software/core/mpl/quaternion_supervisor.h
+++ b/libsensors_iio/software/core/mpl/quaternion_supervisor.h
@@ -1,27 +1,26 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_QUATERNION_SUPERVISOR_H__
-#define INV_QUATERNION_SUPERVISOR_H__
-
-#include "mltypes.h"
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-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
-}
-#endif
-
-#endif // INV_QUATERNION_SUPERVISOR_H__
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_QUATERNION_SUPERVISOR_H__
+#define INV_QUATERNION_SUPERVISOR_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/libsensors_iio/software/core/mpl/sensor_moments.h b/libsensors_iio/software/core/mpl/sensor_moments.h
new file mode 100644
index 0000000..73eb363
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/sensor_moments.h
@@ -0,0 +1,42 @@
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef MLDMP_SENSOR_MOMENTS_H__
+#define MLDMP_SENSOR_MOMENTS_H__
+
+#include "mltypes.h"
+
+enum moment_ord {
+ SECOND_ORD=0,
+ THIRD_ORD,
+ FOURTH_ORD,
+ MAX_ORD
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ inv_error_t inv_enable_sm(void);
+ inv_error_t inv_disable_sm(void);
+ inv_error_t inv_sm_record_data(float sample, void *sensor);
+ inv_error_t inv_sm_update_evt_act_state(int motion);
+ void *inv_init_sm(enum moment_ord);
+ float inv_sm_get_filtered_data(void *sensor);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MLDMP_SENSOR_MOMENTS_H__
+
diff --git a/libsensors_iio/software/core/mpl/shake.h b/libsensors_iio/software/core/mpl/shake.h
deleted file mode 100644
index 8775a4c..0000000
--- a/libsensors_iio/software/core/mpl/shake.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_SHAKE_H__
-#define INV_SHAKE_H__
-
-#include "mltypes.h"
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- #define STATE_ZERO 0
- #define STATE_INIT_1 1
- #define STATE_INIT_2 2
- #define STATE_DETECT 3
-
- struct t_shake_config_params {
- long shake_time_min_ms;
- long shake_time_max_ms;
- long shake_time_min;
- long shake_time_max;
- unsigned char shake_time_set;
- long shake_time_saved;
- float shake_deriv_thr;
- int zero_cross_thr;
- float accel_delta_min;
- float accel_delta_max;
- unsigned char interp_enable;
- };
-
- struct t_shake_state_params {
- unsigned char state;
- float accel_peak_high;
- float accel_peak_low;
- float accel_range;
- int num_zero_cross;
- short curr_shake_time;
- int deriv_major_change;
- int deriv_major_sign;
- float accel_buffer[200];
- float delta_buffer[200];
- };
-
- struct t_shake_data_params {
- float accel_prev;
- float accel_curr;
- float delta_prev;
- float delta_curr;
- float delta_prev_buffer;
- };
-
- struct t_shake_results {
- //unsigned char shake_int;
- int shake_number;
- };
-
- struct t_shake_cb {
- void (*shake_callback)(struct t_shake_results *shake_results);
- };
-
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
- inv_error_t inv_enable_shake(void);
- inv_error_t inv_disable_shake(void);
- inv_error_t inv_init_shake(void);
- inv_error_t inv_start_shake(void);
- int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
- void inv_config_shake_time_params(long sample_time_ms);
- void inv_set_shake_accel_delta_min(float accel_g);
- void inv_set_shake_accel_delta_max(float accel_g);
- void inv_set_shake_zero_cross_thresh(int num_zero_cross);
- void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
- void inv_set_shake_time_min_ms(long time_ms);
- void inv_set_shake_time_max_ms(long time_ms);
- void inv_enable_shake_data_interpolation(unsigned char en);
-
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_SHAKE__
\ No newline at end of file
diff --git a/libsensors_iio/software/core/mpl/state_storage.h b/libsensors_iio/software/core/mpl/state_storage.h
new file mode 100644
index 0000000..c1eb47b
--- /dev/null
+++ b/libsensors_iio/software/core/mpl/state_storage.h
@@ -0,0 +1,25 @@
+/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_STATE_STORAGE_H__
+#define INV_STATE_STORAGE_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_store_data(const void *data, size_t size, unsigned long module,
+ unsigned long version);
+inv_error_t inv_load_data(void *data, size_t size, unsigned long module,
+ unsigned long version);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_STATE_STORAGE_H__
diff --git a/libsensors_iio/software/simple_apps/common/external_hardware.h b/libsensors_iio/software/simple_apps/common/external_hardware.h
new file mode 100644
index 0000000..55e3b20
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/external_hardware.h
@@ -0,0 +1,156 @@
+/*
+ Accelerometer
+*/
+#define get_accel_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_ADXL34X /* ADI accelerometer */
+struct ext_slave_descr *adxl34x_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr adxl34x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA150 /* Bosch accelerometer */
+struct ext_slave_descr *bma150_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma150_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA222 /* Bosch 222 accelerometer */
+struct ext_slave_descr *bma222_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma222_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_BMA250 /* Bosch accelerometer */
+struct ext_slave_descr *bma250_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr bma250_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_KXSD9 /* Kionix accelerometer */
+struct ext_slave_descr *kxsd9_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr kxsd9_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_KXTF9 /* Kionix accelerometer */
+struct ext_slave_descr *kxtf9_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr kxtf9_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_LIS331DLH /* ST accelerometer */
+struct ext_slave_descr *lis331_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lis331_get_slave_descr
+#endif
+
+
+#ifdef CONFIG_MPU_SENSORS_LIS3DH /* ST accelerometer */
+struct ext_slave_descr *lis3dh_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lis3dh_get_slave_descr
+#endif
+
+/* ST accelerometer in LSM303DLx combo */
+#if defined CONFIG_MPU_SENSORS_LSM303DLX_A
+struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr lsm303dlx_a_get_slave_descr
+#endif
+
+/* MPU6050 Accel */
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
+ defined CONFIG_MPU_SENSORS_MPU6050B1
+struct ext_slave_descr *mpu6050_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mpu6050_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMA8450 /* Freescale accelerometer */
+struct ext_slave_descr *mma8450_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mma8450_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMA845X /* Freescale accelerometer */
+struct ext_slave_descr *mma845x_get_slave_descr(void);
+#undef get_accel_slave_descr
+#define get_accel_slave_descr mma845x_get_slave_descr
+#endif
+
+
+/*
+ Compass
+*/
+#define get_compass_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_AK8975 /* AKM compass */
+struct ext_slave_descr *ak8975_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ak8975_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_AMI30X /* AICHI Steel AMI304/305 compass */
+struct ext_slave_descr *ami30x_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ami30x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_AMI306 /* AICHI Steel AMI306 compass */
+struct ext_slave_descr *ami306_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr ami306_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HMC5883 /* Honeywell compass */
+struct ext_slave_descr *hmc5883_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hmc5883_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_MMC314X /* MEMSIC compass */
+struct ext_slave_descr *mmc314x_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr mmc314x_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_LSM303DLX_M /* ST compass */
+struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr lsm303dlx_m_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_YAS529 /* Yamaha compass */
+struct ext_slave_descr *yas529_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr yas529_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_YAS530 /* Yamaha compass */
+struct ext_slave_descr *yas530_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr yas530_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HSCDTD002B /* Alps HSCDTD002B compass */
+struct ext_slave_descr *hscdtd002b_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hscdtd002b_get_slave_descr
+#endif
+
+#ifdef CONFIG_MPU_SENSORS_HSCDTD004A /* Alps HSCDTD004A compass */
+struct ext_slave_descr *hscdtd004a_get_slave_descr(void);
+#undef get_compass_slave_descr
+#define get_compass_slave_descr hscdtd004a_get_slave_descr
+#endif
+/*
+ Pressure
+*/
+#define get_pressure_slave_descr NULL
+
+#ifdef CONFIG_MPU_SENSORS_BMA085 /* BMA pressure */
+struct ext_slave_descr *bma085_get_slave_descr(void);
+#undef get_pressure_slave_descr
+#define get_pressure_slave_descr bma085_get_slave_descr
+#endif
diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.c b/libsensors_iio/software/simple_apps/common/fopenCMake.c
new file mode 100644
index 0000000..2936109
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/fopenCMake.c
@@ -0,0 +1,56 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/******************************************************************************
+ *
+ * $Id: fopenCMake.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <string.h>
+
+#include "fopenCMake.h"
+#include "path_configure.h"
+
+/**
+ * @brief Replacement for fopen that concatenates the location of the
+ * source tree onto the filename path.
+ * It looks in 3 locations:
+ * - in the current directory,
+ * - then it looks in "..",
+ * - lastly in the define UNITTEST_SOURCE_DIR which
+ * gets defined by CMake.
+ * @param filename
+ * Filename relative to base of source directory.
+ * @param prop
+ * Second argument to fopen.
+ */
+FILE *fopenCMake(const char *filename, const char *prop)
+{
+ char path[150];
+ FILE *file;
+
+ // Look first in current directory
+ file = fopen(filename, prop);
+ if (file == NULL) {
+ // Now look in ".."
+#ifdef WIN32
+ strcpy(path, "..\\");
+#else
+ strcpy(path, "../");
+#endif
+ strcat(path, filename);
+ file = fopen(path, prop);
+ if (file == NULL) {
+ // Now look in definition by CMake
+ strcpy(path, PATH_SOURCE_DIR);
+ strcat(path, filename);
+ file = fopen(path, prop);
+ }
+ }
+ return file;
+}
+
+
diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.h b/libsensors_iio/software/simple_apps/common/fopenCMake.h
new file mode 100644
index 0000000..c5eba39
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/fopenCMake.h
@@ -0,0 +1,21 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+#ifndef FOPEN_CMAKE_H__
+#define FOPEN_CMAKE_H__
+
+#include <stdio.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+FILE *fopenCMake( const char *filename, const char *prop );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // FOPEN_CMAKE_H__
diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.c b/libsensors_iio/software/simple_apps/common/gestureMenu.c
new file mode 100644
index 0000000..2a9487c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/gestureMenu.c
@@ -0,0 +1,725 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $
+ *****************************************************************************/
+
+#define _USE_MATH_DEFINES
+#include <stdio.h>
+#include <stddef.h>
+#include <math.h>
+#include <string.h>
+
+#include "ml.h"
+#include "mlmath.h"
+#include "gesture.h"
+#include "orientation.h"
+#include "gestureMenu.h"
+#include "fifo.h"
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "gest"
+#include "log.h"
+#include "mldl_cfg.h"
+
+static unsigned long sensors[] = {
+ INV_NINE_AXIS,
+ INV_THREE_AXIS_GYRO,
+ INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL,
+ INV_THREE_AXIS_ACCEL,
+ INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS,
+ INV_THREE_AXIS_COMPASS,
+ INV_SIX_AXIS_GYRO_ACCEL,
+ INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS,
+ INV_SIX_AXIS_ACCEL_COMPASS,
+};
+
+static char *sensors_string[] = {
+ "INV_NINE_AXIS",
+ "INV_THREE_AXIS_GYRO",
+ "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL",
+ "INV_THREE_AXIS_ACCEL",
+ "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS",
+ "INV_THREE_AXIS_COMPASS",
+ "INV_SIX_AXIS_GYRO_ACCEL",
+ "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS",
+ "INV_SIX_AXIS_ACCEL_COMPASS",
+};
+
+/**
+ * Prints the menu with the current thresholds
+ *
+ * @param params The parameters to print
+ */
+void PrintGestureMenu(tGestureMenuParams const * const params)
+{
+ MPL_LOGI("Press h at any time to re-display this menu\n");
+ MPL_LOGI("TAP PARAMETERS:\n");
+ MPL_LOGI(" Use LEFT and RIGHT arrows to adjust Tap Time \n\n");
+ MPL_LOGI(" j : Increase X threshold : %5d\n",
+ params->xTapThreshold);
+ MPL_LOGI(" J (Shift-j): Decrease X threshold\n");
+ MPL_LOGI(" k : Increase Y threshold : %5d\n",
+ params->yTapThreshold);
+ MPL_LOGI(" K (Shift-k): Decrease Y threshold\n");
+ MPL_LOGI(" i : Increase Z threshold : %5d\n",
+ params->zTapThreshold);
+ MPL_LOGI(" I (Shift-i): Decrease Z threshold\n");
+ MPL_LOGI(" l : Increase tap time : %5d\n",
+ params->tapTime);
+ MPL_LOGI(" L (Shift-l): Decrease tap time\n");
+ MPL_LOGI(" o : Increase next tap time : %5d\n",
+ params->nextTapTime);
+ MPL_LOGI(" O (Shift-o): Increase next tap time\n");
+ MPL_LOGI(" u : Increase max Taps : %5d\n",
+ params->maxTaps);
+ MPL_LOGI(" U (Shift-u): Increase max Taps\n");
+
+ MPL_LOGI("SHAKE PARAMETERS:\n");
+ MPL_LOGI(" x : Increase X threshold : %5d\n",
+ params->xShakeThresh);
+ MPL_LOGI(" X (Shift-x): Decrease X threshold\n");
+ MPL_LOGI(" y : Increase Y threshold : %5d\n",
+ params->yShakeThresh);
+ MPL_LOGI(" Y (Shift-y): Decrease Y threshold\n");
+ MPL_LOGI(" z : Increase Z threshold : %5d\n",
+ params->zShakeThresh);
+ MPL_LOGI(" Z (Shift-z): Decrease Z threshold\n");
+ MPL_LOGI(" s : Toggle Shake Function : %5d\n",
+ params->shakeFunction);
+ MPL_LOGI(" t : Increase Shake Time : %5d\n",
+ params->shakeTime);
+ MPL_LOGI(" T (Shift-T): Decrease Shake Time\n");
+ MPL_LOGI(" n : Increase Next Shake Time : %5d\n",
+ params->nextShakeTime);
+ MPL_LOGI(" N (Shift-n): Decrease Next Shake Time\n");
+ MPL_LOGI(" m : Increase max Shakes : %5d\n",
+ params->maxShakes);
+ MPL_LOGI(" M (Shift-m): Decrease max Shakes\n");
+ MPL_LOGI("SNAP PARAMETERS:\n");
+ MPL_LOGI(" p : Increase Pitch threshold : %5d\n",
+ params->xSnapThresh);
+ MPL_LOGI(" P (Shift-p): Decrease Pitch threshold\n");
+ MPL_LOGI(" r : Increase Roll threshold : %5d\n",
+ params->ySnapThresh);
+ MPL_LOGI(" R (Shift-r): Decrease Roll threshold\n");
+ MPL_LOGI(" a : Increase yAw threshold : %5d\n",
+ params->zSnapThresh);
+ MPL_LOGI(" A (Shift-a): Decrease yAw threshold\n");
+ MPL_LOGI("YAW ROTATION PARAMETERS:\n");
+ MPL_LOGI(" e : Increase yaW Rotate time : %5d\n",
+ params->yawRotateTime);
+ MPL_LOGI(" E (Shift-r): Decrease yaW Rotate time\n");
+ MPL_LOGI(" w : Increase yaW Rotate threshold : %5d\n",
+ params->yawRotateThreshold);
+ MPL_LOGI(" W (Shift-w): Decrease yaW Rotate threshold\n");
+ MPL_LOGI("ORIENTATION PARAMETER:\n");
+ MPL_LOGI(" d : Increase orientation angle threshold : %5f\n",
+ params->orientationThreshold);
+ MPL_LOGI(" D (Shift-d): Decrease orientation angle threshold\n");
+ MPL_LOGI("FIFO RATE:\n");
+ MPL_LOGI(" f : Increase fifo divider : %5d\n",
+ inv_get_fifo_rate());
+ MPL_LOGI(" F (Shift-f): Decrease fifo divider\n");
+ MPL_LOGI("REQUESTED SENSORS:\n");
+ MPL_LOGI(" S (Shift-s): Toggle in use sensors : %s\n",
+ sensors_string[params->sensorsIndex]);
+ MPL_LOGI(" F (Shift-f): Decrease fifo divider\n");
+
+ /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */
+ /* S is available */
+
+ MPL_LOGI("\n\n");
+}
+
+/**
+ * Handles a keyboard input and updates an appropriate threshold, prints then
+ * menu or returns false if the character is not processed.
+ *
+ * @param params The parameters to modify if the thresholds are updated
+ * @param ch The input character
+ *
+ * @return true if the character was processed, false otherwise
+ */
+inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch)
+{
+ int result = INV_SUCCESS;
+ /* Dynamic keyboard processing */
+
+ switch (ch) {
+ case 'j':
+ params->xTapThreshold += 20;
+ // Intentionally fall through
+ case 'J': {
+ params->xTapThreshold -= 10;
+ if (params->xTapThreshold < 0) params->xTapThreshold = 0;
+ result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n",
+ params->xTapThreshold);
+ } break;
+ case 'k':
+ params->yTapThreshold += 20;
+ // Intentionally fall through
+ case 'K': {
+ params->yTapThreshold -= 10;
+ if (params->yTapThreshold < 0) params->yTapThreshold = 0;
+ result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n",
+ params->yTapThreshold);
+ } break;
+ case 'i':
+ params->zTapThreshold += 20;
+ // Intentionally fall through
+ case 'I': {
+ params->zTapThreshold -= 10;
+ if (params->zTapThreshold < 0) params->zTapThreshold = 0;
+ result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("MLSetTapThresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n",
+ params->zTapThreshold);
+ } break;
+
+ case 'l':
+ params->tapTime += 20;
+ // Intentionally fall through
+ case 'L': {
+ params->tapTime -= 10;
+ if (params->tapTime < 0) params->tapTime = 0;
+ result = inv_set_next_tap_time(params->tapTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_next_tap_time returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime);
+ } break;
+ case 'o':
+ params->nextTapTime += 20;
+ // Intentionally fall through
+ case 'O': {
+ params->nextTapTime -= 10;
+ if (params->nextTapTime < 0) params->nextTapTime = 0;
+ result = MLSetNextTapTime(params->nextTapTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
+ }
+ MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime);
+ } break;
+ case 'u':
+ params->maxTaps += 2;
+ // Intentionally fall through
+ case 'U': {
+ params->maxTaps -= 1;
+ if (params->maxTaps < 0) params->maxTaps = 0;
+ result = inv_set_max_taps(params->maxTaps);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_max_taps returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps);
+ } break;
+ case 's': {
+ int shakeParam;
+ params->shakeFunction = (params->shakeFunction + 1) % 2;
+ switch (params->shakeFunction)
+ {
+ case 0:
+ shakeParam = INV_NO_RETRACTION;
+ MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n");
+ break;
+ case 1:
+ shakeParam = INV_RETRACTION;
+ MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n");
+ break;
+ };
+ result = inv_set_shake_func(shakeParam);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_func returned :%d\n", result);
+ }
+ } break;
+ case 'x':
+ params->xShakeThresh += 200;
+ // Intentionally fall through
+ case 'X': {
+ params->xShakeThresh -= 100;
+ result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh);
+ } break;
+ case 'y':
+ params->yShakeThresh += 200;
+ // Intentionally fall through
+ case 'Y': {
+ params->yShakeThresh -= 100;
+ result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh);
+ } break;
+ case 'z':
+ params->zShakeThresh += 200;
+ // Intentionally fall through
+ case 'Z':{
+ params->zShakeThresh -= 100;
+ result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh);
+ } break;
+ case 'r':
+ params->ySnapThresh += 20;
+ // Intentionally fall through
+ case 'R': {
+ params->ySnapThresh -= 10;
+ result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh);
+ } break;
+ case 'p':
+ params->xSnapThresh += 20;
+ // Intentionally fall through
+ case 'P': {
+ params->xSnapThresh -= 10;
+ result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n",
+ params->xSnapThresh);
+ } break;
+ case 'a':
+ params->zSnapThresh += 20;
+ case 'A': {
+ params->zSnapThresh -= 10;
+ result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh);
+ } break;
+
+ case 't':
+ params->shakeTime += 20;
+ case 'T':{
+ params->shakeTime -= 10;
+ result = inv_set_shake_time(params->shakeTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_time returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime);
+ } break;
+ case 'n':
+ params->nextShakeTime += 20;
+ case 'N':{
+ params->nextShakeTime -= 10;
+ result = inv_set_next_shake_time(params->nextShakeTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_next_shake_time returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime);
+ } break;
+ case 'm':
+ params->maxShakes += 2;
+ case 'M':{
+ params->maxShakes -= 1;
+ result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_max_shakes returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes);
+ } break;
+ case 'e':
+ params->yawRotateTime += 20;
+ case 'E':{
+ params->yawRotateTime -= 10;
+ result = inv_set_yaw_rotate_time(params->yawRotateTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime);
+ } break;
+ case 'w':
+ params->yawRotateThreshold += 2;
+ case 'W':{
+ params->yawRotateThreshold -= 1;
+ result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold);
+ } break;
+ case 'c':
+ params->shakeRejectValue += 0.20f;
+ case 'C':{
+ params->shakeRejectValue -= 0.10f;
+ result = inv_set_tap_shake_reject(params->shakeRejectValue);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue);
+ } break;
+ case 'd':
+ params->orientationThreshold += 10;
+ case 'D':{
+ params->orientationThreshold -= 5;
+ if (params->orientationThreshold > 90) {
+ params->orientationThreshold = 90;
+ }
+
+ if (params->orientationThreshold < 0 ) {
+ params->orientationThreshold = 0;
+ }
+
+ result = inv_set_orientation_thresh(params->orientationThreshold,
+ 5, 80,
+ INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result);
+ }
+ MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d,"
+ " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n",
+ params->orientationThreshold, 5, 80);
+ } break;
+ case 'f':
+ result = inv_set_fifo_rate(inv_get_fifo_rate() + 1);
+ MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate());
+ break;
+ case 'F':
+ {
+ unsigned short newRate = inv_get_fifo_rate();
+ if (newRate > 0)
+ newRate--;
+ result = inv_set_fifo_rate(newRate);
+ MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate());
+ break;
+ }
+ case 'S':
+ params->sensorsIndex++;
+ if (params->sensorsIndex >= ARRAY_SIZE(sensors)) {
+ params->sensorsIndex = 0;
+ }
+ result = inv_set_mpu_sensors(
+ sensors[params->sensorsIndex] & params->available_sensors);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result,
+ sensors_string[params->sensorsIndex]);
+ break;
+ case 'h':
+ case 'H': {
+ PrintGestureMenu(params);
+ } break;
+ default: {
+ result = INV_ERROR;
+ } break;
+ };
+ return result;
+}
+
+/**
+ * Initializes the tGestureMenuParams to a set of defaults.
+ *
+ * @param params The parameters to initialize.
+ */
+void GestureMenuSetDefaults(tGestureMenuParams * const params)
+{
+ params->xTapThreshold = 100;
+ params->yTapThreshold = 100;
+ params->zTapThreshold = 100;
+ params->tapTime = 100;
+ params->nextTapTime = 600;
+ params->maxTaps = 2;
+ params->shakeRejectValue = 0.8f;
+ params->xShakeThresh = 750;
+ params->yShakeThresh = 750;
+ params->zShakeThresh = 750;
+ params->xSnapThresh = 160;
+ params->ySnapThresh = 320;
+ params->zSnapThresh = 160;
+ params->shakeTime = 100;
+ params->nextShakeTime = 1000;
+ params->shakeFunction = 0;
+ params->maxShakes = 3;
+ params->yawRotateTime = 80;
+ params->yawRotateThreshold = 70;
+ params->orientationThreshold = 60;
+ params->sensorsIndex = 0;
+ params->available_sensors = INV_NINE_AXIS;
+}
+
+void GestureMenuSetAvailableSensors(tGestureMenuParams * const params,
+ unsigned long available_sensors)
+{
+ params->available_sensors = available_sensors;
+}
+/**
+ * Call the appropriate MPL set threshold functions and checkes the error codes
+ *
+ * @param params The parametrs to use in setting the thresholds
+ *
+ * @return INV_SUCCESS or the first error code encountered.
+ */
+inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params)
+{
+ inv_error_t result = INV_SUCCESS;
+
+ result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_tap_threshold returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_next_tap_time(params->tapTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_next_tap_time returned :%d\n", result);
+ return result;
+ }
+ result = MLSetNextTapTime(params->nextTapTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_max_taps(params->maxTaps);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_max_taps returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_tap_shake_reject(params->shakeRejectValue);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result);
+ return result;
+ }
+
+ //Set up shake gesture
+ result = inv_set_shake_func(params->shakeFunction);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_func returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_thresh returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_shake_time(params->shakeTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_shake_time returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_next_shake_time(params->nextShakeTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_next_shake_time returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_max_shakes returned :%d\n", result);
+ return result;
+ }
+
+ // Yaw rotate settings
+ result = inv_set_yaw_rotate_time(params->yawRotateTime);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result);
+ return result;
+ }
+ result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result);
+ return result;
+ }
+
+ // Orientation settings
+ result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80,
+ INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result);
+ return result;
+ }
+
+ // Requested Sensors
+ result = inv_set_mpu_sensors(
+ sensors[params->sensorsIndex] & params->available_sensors);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result,
+ sensors[params->sensorsIndex] & params->available_sensors);
+ return result;
+ }
+
+ return INV_SUCCESS;
+}
+
+void PrintGesture(tGesture* gesture)
+{
+ float speed;
+ char type[1024];
+ switch (gesture->type)
+ {
+ case INV_TAP:
+ {
+ if (gesture->meta < 0) {
+ snprintf(type,sizeof(type),"-");
+ } else {
+ snprintf(type,sizeof(type),"+");
+ }
+
+ switch (ABS(gesture->meta))
+ {
+ case 1:
+ strcat(type,"X");
+ break;
+ case 2:
+ strcat(type,"Y");
+ break;
+ case 3:
+ strcat(type,"Z");
+ break;
+ default:
+ strcat(type,"ERROR");
+ break;
+ };
+ MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n",
+ type,
+ gesture->num,
+ gesture->strength,
+ gesture->speed,
+ gesture->reserved,
+ (180 / M_PI) * atan2(
+ (float)gesture->strength, (float)gesture->speed),
+ (180 / M_PI) * atan2(
+ (float)gesture->speed, (float)gesture->reserved),
+ (180 / M_PI) * atan2(
+ (float)gesture->strength, (float)gesture->reserved)
+ );
+ }
+ break;
+ case INV_ROLL_SHAKE:
+ case INV_PITCH_SHAKE:
+ case INV_YAW_SHAKE:
+ {
+ if (gesture->strength){
+ snprintf(type, sizeof(type), "Snap : ");
+ } else {
+ snprintf(type, sizeof(type), "Shake: ");
+ }
+
+ if (gesture->meta==0) {
+ strcat(type, "+");
+ } else {
+ strcat(type, "-");
+ }
+
+ if (gesture->type == INV_ROLL_SHAKE) {
+ strcat(type, "Roll ");
+ } else if (gesture->type == INV_PITCH_SHAKE) {
+ strcat(type, "Pitch ");
+ } else if (gesture->type == INV_YAW_SHAKE) {
+ strcat(type, "Yaw ");
+ }
+
+ speed = (float)gesture->speed +
+ (float)(gesture->reserved / (float)(1 << 16));
+ MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed);
+ }
+ break;
+ case INV_YAW_IMAGE_ROTATE:
+ {
+ if (gesture->meta == 0) {
+ snprintf(type, sizeof(type), "Positive ");
+ } else {
+ snprintf(type, sizeof(type), "Negative ");
+ }
+ MPL_LOGI("%s Yaw Image Rotation\n", type);
+ }
+ break;
+ default:
+ MPL_LOGE("Unknown Gesture received\n");
+ break;
+ }
+}
+
+/**
+ * Prints the new or current orientation using MPL_LOGI and remembers the last
+ * orientation to print orientation flips.
+ *
+ * @param orientation the new or current orientation. 0 to reset.
+ */
+void PrintOrientation(unsigned short orientation)
+{
+ // Determine if it was a flip
+ static int sLastOrientation = 0;
+ int flip = orientation | sLastOrientation;
+
+ if ((INV_X_UP | INV_X_DOWN) == flip) {
+ MPL_LOGI("Flip about the X Axis: \n");
+ } else if ((INV_Y_UP | INV_Y_DOWN) == flip) {
+ MPL_LOGI("Flip about the Y axis: \n");
+ } else if ((INV_Z_UP | INV_Z_DOWN) == flip) {
+ MPL_LOGI("Flip about the Z axis: \n");
+ }
+ sLastOrientation = orientation;
+
+ switch (orientation) {
+ case INV_X_UP:
+ MPL_LOGI("X Axis is up\n");
+ break;
+ case INV_X_DOWN:
+ MPL_LOGI("X Axis is down\n");
+ break;
+ case INV_Y_UP:
+ MPL_LOGI("Y Axis is up\n");
+ break;
+ case INV_Y_DOWN:
+ MPL_LOGI("Y Axis is down\n");
+ break;
+ case INV_Z_UP:
+ MPL_LOGI("Z Axis is up\n");
+ break;
+ case INV_Z_DOWN:
+ MPL_LOGI("Z Axis is down\n");
+ break;
+ case 0:
+ break; /* Not an error. Resets sLastOrientation */
+ default:
+ MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation);
+ break;
+ }
+}
+
+
diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.h b/libsensors_iio/software/simple_apps/common/gestureMenu.h
new file mode 100644
index 0000000..8f804e1
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/gestureMenu.h
@@ -0,0 +1,75 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/***************************************************************************** *
+ * $Id: gestureMenu.h 5705 2011-06-28 19:32:29Z nroyer $
+ ******************************************************************************/
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file gestureMenu.h
+ * @brief
+ *
+ *
+ */
+
+#ifndef __GESTUREMENU_H__
+#define __GESTUREMENU_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/******************************************************************************/
+ typedef struct sGestureMenuParams {
+ /* Tap Params */
+ int xTapThreshold;
+ int yTapThreshold;
+ int zTapThreshold;
+ int tapTime;
+ int nextTapTime;
+ int maxTaps;
+ float shakeRejectValue;
+
+ /* Shake Params */
+ int xShakeThresh;
+ int yShakeThresh;
+ int zShakeThresh;
+ int xSnapThresh;
+ int ySnapThresh;
+ int zSnapThresh;
+ int shakeTime;
+ int nextShakeTime;
+ int shakeFunction;
+ int maxShakes;
+
+ /* Yaw rotate params */
+ int yawRotateTime;
+ int yawRotateThreshold;
+
+ /* Orientation */
+ float orientationThreshold;
+ int sensorsIndex;
+ unsigned long available_sensors;
+ } tGestureMenuParams;
+
+ void PrintGestureMenu(tGestureMenuParams const * const params) ;
+ inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params,char ch);
+ void PrintGesture(gesture_t* gesture);
+ void PrintOrientation(unsigned short orientation);
+ void GestureMenuSetDefaults(tGestureMenuParams * const params);
+ void GestureMenuSetAvailableSensors(tGestureMenuParams * const params,
+ unsigned long available_sensors);
+ inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params);
+
+/******************************************************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // __GESTUREMENU_H__
diff --git a/libsensors_iio/software/simple_apps/common/helper.c b/libsensors_iio/software/simple_apps/common/helper.c
new file mode 100644
index 0000000..4d634bd
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/helper.c
@@ -0,0 +1,110 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: helper.c 4367 2010-12-21 03:02:55Z prao $
+ *
+ *******************************************************************************/
+
+#include <stdio.h>
+#ifdef _WIN32
+#include <windows.h>
+#include <conio.h>
+#endif
+#ifdef LINUX
+#include <sys/select.h>
+#endif
+#include <time.h>
+#include <string.h>
+
+#include "ml.h"
+#include "slave.h"
+#include "mldl.h"
+#include "mltypes.h"
+#include "mlstates.h"
+#include "compass.h"
+
+#include "mlsl.h"
+#include "ml.h"
+
+#include "helper.h"
+#include "mlsetup.h"
+#include "fopenCMake.h"
+#include "int.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-helper"
+
+#ifdef AIO
+extern inv_error_t MLSLSetYamahaCompassDataMode(unsigned char mode);
+#endif
+
+// Keyboard hit function
+int ConsoleKbhit(void)
+{
+#ifdef _WIN32
+ return _kbhit();
+#else
+ struct timeval tv;
+ fd_set read_fd;
+
+ tv.tv_sec=0;
+ tv.tv_usec=0;
+ FD_ZERO(&read_fd);
+ FD_SET(0,&read_fd);
+
+ if(select(1, &read_fd, NULL, NULL, &tv) == -1)
+ return 0;
+
+ if(FD_ISSET(0,&read_fd))
+ return 1;
+
+ return 0;
+#endif
+}
+
+char ConsoleGetChar(void) {
+#ifdef _WIN32
+ return _getch();
+#else
+ return getchar();
+#endif
+}
+struct mpuirq_data** InterruptPoll(int *handles, int numHandles, long tv_sec, long tv_usec)
+{
+ struct mpuirq_data **data;
+ void *tmp;
+ int ii;
+ const int irq_data_size = sizeof(**data) * numHandles +
+ sizeof(*data) * numHandles;
+
+ tmp = (void *)inv_malloc(irq_data_size);
+ memset(tmp, 0, irq_data_size);
+ data = (struct mpuirq_data **)tmp;
+ for (ii = 0; ii < numHandles; ii++) {
+ data[ii] = (struct mpuirq_data *)((unsigned long)tmp +
+ (sizeof(*data) * numHandles) + sizeof(**data) * ii);
+ }
+
+ if (IntProcess(handles, numHandles, data, tv_sec, tv_usec) > 0) {
+ for (ii = 0; ii < numHandles; ii++) {
+ if (data[ii]->interruptcount) {
+ inv_interrupt_handler(ii);
+ }
+ }
+ }
+
+ /* Return data incase the application needs to look at the timestamp or
+ other part of the data */
+ return data;
+}
+
+void InterruptPollDone(struct mpuirq_data ** data)
+{
+ inv_free(data);
+}
diff --git a/libsensors_iio/software/simple_apps/common/helper.h b/libsensors_iio/software/simple_apps/common/helper.h
new file mode 100644
index 0000000..b2da520
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/helper.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: helper-customer.h 5770 2011-07-14 01:34:10Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef HELPER_C_H
+#define HELPER_C_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+/*
+ Defines
+*/
+
+#define CALL_N_CHECK(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ } \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ return r35uLt; \
+ } \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ return; \
+ } \
+}
+
+#define CALL_CHECK_N_EXIT(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ exit (r35uLt); \
+ } \
+}
+
+
+#define CALL_CHECK_N_CALLBACK(f, cb) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ cb; \
+ } \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ goto label; \
+ } \
+}
+
+#define DEFAULT_PLATFORM PLATFORM_ID_MSB_V2
+#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9
+#define DEFAULT_COMPASS_ID COMPASS_ID_AK8975
+
+#define DataLogger(x) NULL
+#define DataLoggerSelector(x) //
+#define DataLoggerCb(x) NULL
+#define findComm() (9)
+#define MenuHwChoice(p,a,c) (*p = DEFAULT_PLATFORM, *a = DEFAULT_ACCEL_ID, \
+ *c = DEFAULT_COMPASS_ID, INV_ERROR)
+
+ char ConsoleGetChar(void);
+ int ConsoleKbhit(void);
+ struct mpuirq_data **InterruptPoll(
+ int *handles, int numHandles, long tv_sec, long tv_usec);
+ void InterruptPollDone(struct mpuirq_data ** data);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // HELPER_C_H
diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/libsensors_iio/software/simple_apps/common/mlerrorcode.c
new file mode 100644
index 0000000..25b0df6
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.c
@@ -0,0 +1,96 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/******************************************************************************
+ *
+ * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $
+ *
+ *****************************************************************************/
+
+#include <stdio.h>
+#include <string.h>
+
+#include "mltypes.h"
+#include "mlerrorcode.h"
+
+#define ERROR_CODE_CASE(CODE) \
+ case CODE: \
+ return #CODE \
+
+/**
+ * @brief return a string containing the label assigned to the error code.
+ *
+ * @param errorcode
+ * The errorcode value of which the label has to be returned.
+ *
+ * @return A string containing the error code label.
+ */
+char* MLErrorCode(inv_error_t errorcode)
+{
+ switch(errorcode) {
+ ERROR_CODE_CASE(INV_SUCCESS);
+ ERROR_CODE_CASE(INV_ERROR);
+ ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER);
+ ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED);
+ ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED);
+ ERROR_CODE_CASE(INV_ERROR_DMP_STARTED);
+ ERROR_CODE_CASE(INV_ERROR_NOT_OPENED);
+ ERROR_CODE_CASE(INV_ERROR_OPENED);
+ ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE);
+ ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED);
+ ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO);
+ ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE);
+ ERROR_CODE_CASE(INV_ERROR_FILE_OPEN);
+ ERROR_CODE_CASE(INV_ERROR_FILE_READ);
+ ERROR_CODE_CASE(INV_ERROR_FILE_WRITE);
+
+ ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED);
+ ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR);
+ ERROR_CODE_CASE(INV_ERROR_SERIAL_READ);
+ ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE);
+ ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED);
+
+ ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION);
+ ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE);
+
+ ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW);
+ ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER);
+ ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT);
+ ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA);
+ ERROR_CODE_CASE(INV_ERROR_MEMORY_SET);
+
+ ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR);
+ ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR);
+
+ ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR);
+ ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE);
+ ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED);
+ ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED);
+
+ ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW);
+ ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW);
+ ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY);
+ ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR);
+
+ ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD);
+ ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE);
+ ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN);
+ ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM);
+
+ default:
+ {
+ #define UNKNOWN_ERROR_CODE 1234
+ return ERROR_NAME(UNKNOWN_ERROR_CODE);
+ break;
+ }
+
+ }
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/libsensors_iio/software/simple_apps/common/mlerrorcode.h
new file mode 100644
index 0000000..9a35792
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.h
@@ -0,0 +1,86 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $
+ *
+ *******************************************************************************/
+
+#ifndef _MLERRORCODE_H_
+#define _MLERRORCODE_H_
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ Defines
+*/
+#define CALL_N_CHECK(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ } \
+}
+
+#define CALL_CHECK_N_RETURN_ERROR(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ return r35uLt; \
+ } \
+}
+
+// for functions returning void
+#define CALL_CHECK_N_RETURN(f) do { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ return; \
+ } \
+ } while(0)
+
+#define CALL_CHECK_N_EXIT(f) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ exit (r35uLt); \
+ } \
+}
+
+
+#define CALL_CHECK_N_CALLBACK(f, cb) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ cb; \
+ } \
+}
+
+#define CALL_CHECK_N_GOTO(f, label) { \
+ unsigned int r35uLt = f; \
+ if(INV_SUCCESS != r35uLt) { \
+ MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \
+ __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \
+ goto label; \
+ } \
+}
+
+char* MLErrorCode(inv_error_t errorcode);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c
new file mode 100644
index 0000000..f11bce9
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlsetup.c
@@ -0,0 +1,1722 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/******************************************************************************
+ *
+ * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $
+ *
+ *****************************************************************************/
+#undef MPL_LOG_NDEBUG
+#ifdef UNITTESTING
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+
+/**
+ * @defgroup MLSETUP
+ * @brief The Motion Library external slaves setup override suite.
+ *
+ * Use these APIs to override the kernel/default settings in the
+ * corresponding data structures for gyros, accel, and compass.
+ *
+ * @{
+ * @file mlsetup.c
+ * @brief The Motion Library external slaves setup override suite.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+/*
+ Defines
+*/
+/* these have to appear before inclusion of mpu.h */
+#define CONFIG_MPU_SENSORS_KXSD9 y // Kionix accel
+#define CONFIG_MPU_SENSORS_KXTF9 y // Kionix accel
+#define CONFIG_MPU_SENSORS_LIS331DLH y // ST accelerometer
+#define CONFIG_MPU_SENSORS_LSM303DLX_A y // ST accelerometer in LSM303DLx combo
+#define CONFIG_MPU_SENSORS_LIS3DH y // ST accelerometer
+#define CONFIG_MPU_SENSORS_BMA150 y // Bosch 150 accelerometer
+#define CONFIG_MPU_SENSORS_BMA222 y // Bosch 222 accelerometer
+#define CONFIG_MPU_SENSORS_BMA250 y // Bosch 250 accelerometer
+#define CONFIG_MPU_SENSORS_ADXL34X y // AD 345 or 346 accelerometer
+#define CONFIG_MPU_SENSORS_MMA8450 y // Freescale MMA8450 accelerometer
+#define CONFIG_MPU_SENSORS_MMA845X y // Freescale MMA845X accelerometer
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y // Invensense MPU6050 built-in accelerometer
+#endif
+
+#define CONFIG_MPU_SENSORS_AK8975 y // AKM compass
+#define CONFIG_MPU_SENSORS_AMI30X y // AICHI AMI304/305 compass
+#define CONFIG_MPU_SENSORS_AMI306 y // AICHI AMI306 compass
+#define CONFIG_MPU_SENSORS_HMC5883 y // Honeywell compass
+#define CONFIG_MPU_SENSORS_LSM303DLX_M y // ST compass in LSM303DLx combo
+#define CONFIG_MPU_SENSORS_YAS529 y // Yamaha compass
+#define CONFIG_MPU_SENSORS_YAS530 y // Yamaha compass
+#define CONFIG_MPU_SENSORS_MMC314X y // MEMSIC compass
+#define CONFIG_MPU_SENSORS_HSCDTD002B y // ALPS compass
+#define CONFIG_MPU_SENSORS_HSCDTD004A y // ALPS HSCDTD004A compass
+
+#define CONFIG_MPU_SENSORS_BMA085 y // Bosch 085 pressure
+
+#include "external_hardware.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#include "slave.h"
+#include "compass.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-mlsetup"
+
+#include "linux/mpu.h"
+
+#include "mlsetup.h"
+
+#ifdef LINUX
+#include "errno.h"
+#endif
+
+/* Override these structures from mldl.c */
+extern struct ext_slave_descr g_slave_accel;
+extern struct ext_slave_descr g_slave_compass;
+//extern struct ext_slave_descr g_slave_pressure;
+/* Platform Data */
+//extern struct mpu_platform_data g_pdata;
+extern struct ext_slave_platform_data g_pdata_slave_accel;
+extern struct ext_slave_platform_data g_pdata_slave_compass;
+//extern struct ext_slave_platform_data g_pdata_slave_pressure;
+signed char g_gyro_orientation[9];
+
+/*
+ Typedefs
+*/
+typedef void tSetupFuncAccel(void);
+typedef void tSetupFuncCompass(void);
+typedef void tSetupFuncPressure(void);
+
+#ifdef LINUX
+#include <sys/ioctl.h>
+#endif
+
+/*********************************************************************
+ Dragon - PLATFORM_ID_DRAGON_PROTOTYPE
+*********************************************************************/
+/**
+ * @internal
+ * @brief performs a 180' rotation around Z axis to reflect
+ * usage of the multi sensor board (MSB) with the
+ * beagleboard
+ * @note assumes well formed mounting matrix, with only
+ * one 1 for each row.
+ */
+static void Rotate180DegAroundZAxis(signed char matrix[])
+{
+ int ii;
+ for(ii=0; ii<6; ii++) {
+ matrix[ii] = -matrix[ii];
+ }
+}
+
+/**
+ * @internal
+ * Sets the orientation based on the position of the mounting. For different
+ * devices the relative position to pin 1 will be different.
+ *
+ * Positions are:
+ * - 0-3 are Z up
+ * - 4-7 are Z down
+ * - 8-11 are Z right
+ * - 12-15 are Z left
+ * - 16-19 are Z front
+ * - 20-23 are Z back
+ *
+ * @param position The position of the orientation
+ * @param orientation the location to store the new oreintation
+ */
+static inv_error_t SetupOrientation(unsigned int position,
+ signed char *orientation)
+{
+ memset(orientation, 0, 9);
+ switch (position){
+ case 0:
+ /*-------------------------*/
+ orientation[0] = +1;
+ orientation[4] = +1;
+ orientation[8] = +1;
+ break;
+ case 1:
+ /*-------------------------*/
+ orientation[1] = +1;
+ orientation[3] = -1;
+ orientation[8] = +1;
+ break;
+ case 2:
+ /*-------------------------*/
+ orientation[0] = -1;
+ orientation[4] = -1;
+ orientation[8] = +1;
+ break;
+ case 3:
+ /*-------------------------*/
+ orientation[1] = -1;
+ orientation[3] = +1;
+ orientation[8] = +1;
+ break;
+ case 4:
+ /*-------------------------*/
+ orientation[0] = -1;
+ orientation[4] = +1;
+ orientation[8] = -1;
+ break;
+ case 5:
+ /*-------------------------*/
+ orientation[1] = -1;
+ orientation[3] = -1;
+ orientation[8] = -1;
+ break;
+ case 6:
+ /*-------------------------*/
+ orientation[0] = +1;
+ orientation[4] = -1;
+ orientation[8] = -1;
+ break;
+ case 7:
+ /*-------------------------*/
+ orientation[1] = +1;
+ orientation[3] = +1;
+ orientation[8] = -1;
+ break;
+ case 8:
+ /*-------------------------*/
+ orientation[2] = +1;
+ orientation[3] = +1;
+ orientation[7] = +1;
+ break;
+ case 9:
+ /*-------------------------*/
+ orientation[2] = +1;
+ orientation[4] = +1;
+ orientation[6] = -1;
+ break;
+ case 10:
+ orientation[2] = +1;
+ orientation[3] = -1;
+ orientation[7] = -1;
+ break;
+ case 11:
+ orientation[2] = +1;
+ orientation[4] = -1;
+ orientation[6] = +1;
+ break;
+ case 12:
+ orientation[2] = -1;
+ orientation[3] = -1;
+ orientation[7] = +1;
+ break;
+ case 13:
+ orientation[2] = -1;
+ orientation[4] = -1;
+ orientation[6] = -1;
+ break;
+ case 14:
+ orientation[2] = -1;
+ orientation[3] = +1;
+ orientation[7] = -1;
+ break;
+ case 15:
+ orientation[2] = -1;
+ orientation[4] = +1;
+ orientation[6] = +1;
+ break;
+ case 16:
+ orientation[0] = -1;
+ orientation[5] = +1;
+ orientation[7] = +1;
+ break;
+ case 17:
+ orientation[1] = -1;
+ orientation[5] = +1;
+ orientation[6] = -1;
+ break;
+ case 18:
+ orientation[0] = +1;
+ orientation[5] = -1;
+ orientation[7] = -1;
+ break;
+ case 19:
+ orientation[1] = -1;
+ orientation[5] = +1;
+ orientation[6] = +1;
+ break;
+ case 20:
+ orientation[0] = +1;
+ orientation[5] = -1;
+ orientation[7] = +1;
+ break;
+ case 21:
+ orientation[1] = -1;
+ orientation[5] = -1;
+ orientation[6] = +1;
+ break;
+ case 22:
+ orientation[0] = -1;
+ orientation[5] = -1;
+ orientation[7] = -1;
+ break;
+ case 23:
+ orientation[1] = +1;
+ orientation[5] = -1;
+ orientation[6] = -1;
+ break;
+ default:
+ MPL_LOGE("Invalid position %d\n", position);
+ LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+
+ return INV_SUCCESS;
+}
+
+static void PrintMountingOrientation(
+ const char * header, signed char *orientation)
+{
+ MPL_LOGV("%s:\n", header);
+ MPL_LOGV("\t[[%3d, %3d, %3d]\n",
+ orientation[0], orientation[1], orientation[2]);
+ MPL_LOGV("\t [%3d, %3d, %3d]\n",
+ orientation[3], orientation[4], orientation[5]);
+ MPL_LOGV("\t [%3d, %3d, %3d]]\n",
+ orientation[6], orientation[7], orientation[8]);
+}
+
+/*****************************
+ * Accel Setup Functions *
+ *****************************/
+
+static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ position = 5;
+ break;
+ case PLATFORM_ID_ST_6AXIS:
+ position = 0;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *lis331_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS331;
+ return INV_SUCCESS;
+}
+
+static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ position = 1;
+ break;
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ position = 3;
+ break;
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *lis3dh_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS3DH;
+ return result;
+}
+
+static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 7;
+ break;
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *kxsd9_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXSD9;
+ return result;
+}
+
+static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB_EVB:
+ position =0;
+ break;
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ position = 7;
+ break;
+#ifdef WIN32
+ case PLATFORM_ID_DONGLE:
+ position = 1;
+ break;
+#endif
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ position = 1;
+ break;
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *kxtf9_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXTF9;
+ return result;
+}
+
+static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ position = 3;
+ break;
+ case PLATFORM_ID_MSB_V2:
+ position = 1;
+ break;
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *lsm303dlx_a_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LSM303;
+ return result;
+}
+
+static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 6;
+ break;
+#ifdef WIN32
+ case PLATFORM_ID_DONGLE:
+ position = 3;
+ break;
+#endif
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *bma150_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA150;
+ return result;
+}
+
+static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 0;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *bma222_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA222;
+ return result;
+}
+
+static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ position = 0;
+ break;
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ position = 3;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *bma250_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA250;
+ return result;
+}
+
+static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 6;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *adxl34x_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_ADXL34X;
+ return result;
+}
+
+
+static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 5;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *mma8450_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA8450;
+ return result;
+}
+
+
+static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 5;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_accel.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_accel = *mma845x_get_slave_descr();
+#endif
+ g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA845X;
+ return result;
+}
+
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the gyro was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_MANTIS_MSB:
+ position = 6;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_DRAGON_USB_DONGLE:
+ position = 1;
+ break;
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ case PLATFORM_ID_MANTIS_EVB:
+ position = 0;
+ break;
+ case PLATFORM_ID_MSB_V3:
+ position = 2;
+ break;
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ return INV_ERROR_INVALID_PARAMETER;
+ };
+
+ SetupOrientation(position, g_pdata_slave_accel.orientation);
+ /* Interrupt */
+#ifndef LINUX
+#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
+ // g_slave_accel = // fixme *mpu6050_get_slave_descr();
+#endif
+#endif
+ g_pdata_slave_accel.address = 0x68;
+ return result;
+}
+
+/*****************************
+ Compass Setup Functions
+******************************/
+static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_MANTIS_MSB:
+ position = 2;
+ break;
+#ifdef WIN32
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ position = 4;
+ break;
+#endif
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ position = 7;
+ break;
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V3:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ position = 6;
+ break;
+ case PLATFORM_ID_DRAGON_USB_DONGLE:
+ case PLATFORM_ID_MSB_EVB:
+ position = 5;
+ break;
+ case PLATFORM_ID_MANTIS_EVB:
+ position = 4;
+ break;
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *ak8975_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AKM;
+ return result;
+}
+
+static inv_error_t SetupCompassMMCCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 7;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *mmc314x_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_MMC314X;
+ return result;
+}
+
+static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 4;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI304;
+#ifndef LINUX
+ g_slave_compass = *ami30x_get_slave_descr();
+#endif
+ return result;
+}
+
+static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ position = 3;
+ break;
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ position = 1;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *ami306_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI306;
+ return result;
+}
+
+static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 6;
+ break;
+#ifdef WIN32
+ case PLATFORM_ID_DONGLE:
+ position = 2;
+ break;
+#endif
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *hmc5883_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883;
+ return result;
+}
+
+
+static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 1;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ };
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+#ifndef LINUX
+ g_slave_compass = *lsm303dlx_m_get_slave_descr();
+ g_slave_compass.id = COMPASS_ID_LSM303DLH;
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883;
+ return result;
+}
+
+static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ position = 8;
+ break;
+ case PLATFORM_ID_MSB_V2:
+ position = 12;
+ break;
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ };
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *lsm303dlx_m_get_slave_descr();
+ g_slave_compass.id = COMPASS_ID_LSM303DLM;
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883;
+ return result;
+}
+
+static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ position = 1;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *yas530_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS530;
+ return result;
+}
+
+static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 6;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *yas529_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS529;
+ return result;
+}
+
+
+static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 2;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *hscdtd002b_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX;
+ return result;
+}
+
+static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId)
+{
+ inv_error_t result = INV_SUCCESS;
+ unsigned int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ position = 1;
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_MANTIS_MSB:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+ return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+ };
+ result = SetupOrientation(position, g_pdata_slave_compass.orientation);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+#ifndef LINUX
+ g_slave_compass = *hscdtd004a_get_slave_descr();
+#endif
+ g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX;
+ return result;
+}
+
+
+/*****************************
+ Pressure Setup Functions
+******************************/
+#if 0
+static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId)
+{
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation));
+
+ g_pdata_slave_pressure.bus = EXT_SLAVE_BUS_PRIMARY;
+#ifndef LINUX
+ g_slave_pressure = *bma085_get_slave_descr();
+#endif
+ g_pdata_slave_pressure.address = PRESSURE_SLAVEADDR_BMA085;
+ return INV_SUCCESS;
+}
+#endif
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupAccelCalibration(unsigned short platformId,
+ unsigned short accelId)
+{
+ /*---- setup the accels ----*/
+ switch(accelId) {
+ case ACCEL_ID_LSM303DLX:
+ SetupAccelLSM303Calibration(platformId);
+ break;
+ case ACCEL_ID_LIS331:
+ SetupAccelSTLIS331Calibration(platformId);
+ break;
+ case ACCEL_ID_KXSD9:
+ SetupAccelKionixKXSD9Calibration(platformId);
+ break;
+ case ACCEL_ID_KXTF9:
+ SetupAccelKionixKXTF9Calibration(platformId);
+ break;
+ case ACCEL_ID_BMA150:
+ SetupAccelBMA150Calibration(platformId);
+ break;
+ case ACCEL_ID_BMA222:
+ SetupAccelBMA222Calibration(platformId);
+ break;
+ case ACCEL_ID_BMA250:
+ SetupAccelBMA250Calibration(platformId);
+ break;
+ case ACCEL_ID_ADXL34X:
+ SetupAccelADXL34XCalibration(platformId);
+ break;
+ case ACCEL_ID_MMA8450:
+ SetupAccelMMA8450Calibration(platformId);
+ break;
+ case ACCEL_ID_MMA845X:
+ SetupAccelMMA845XCalibration(platformId);
+ break;
+ case ACCEL_ID_LIS3DH:
+ SetupAccelSTLIS3DHCalibration(platformId);
+ break;
+ case ACCEL_ID_MPU6050:
+ SetupAccelMPU6050Calibration(platformId);
+ break;
+ case ID_INVALID:
+ break;
+ default:
+ LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+
+ if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) {
+ g_pdata_slave_accel.bus = EXT_SLAVE_BUS_SECONDARY;
+ } else if (accelId != ACCEL_ID_MPU6050) {
+ g_pdata_slave_accel.bus = EXT_SLAVE_BUS_PRIMARY;
+ }
+
+#ifndef WIN32
+ if (accelId != ID_INVALID)
+ Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation);
+#endif
+
+ return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+inv_error_t SetupCompassCalibration(unsigned short platformId,
+ unsigned short compassId)
+{
+ /*---- setup the compass ----*/
+ switch(compassId) {
+ case COMPASS_ID_AK8975:
+ SetupCompassAKM8975Calibration(platformId);
+ break;
+ case COMPASS_ID_AMI30X:
+ SetupCompassAMI304Calibration(platformId);
+ break;
+ case COMPASS_ID_AMI306:
+ SetupCompassAMI306Calibration(platformId);
+ break;
+ case COMPASS_ID_LSM303DLH:
+ SetupCompassLSM303DLHCalibration(platformId);
+ break;
+ case COMPASS_ID_LSM303DLM:
+ SetupCompassLSM303DLMCalibration(platformId);
+ break;
+ case COMPASS_ID_HMC5883:
+ SetupCompassHMC5883Calibration(platformId);
+ break;
+ case COMPASS_ID_YAS529:
+ SetupCompassYAS529Calibration(platformId);
+ break;
+ case COMPASS_ID_YAS530:
+ SetupCompassYAS530Calibration(platformId);
+ break;
+ case COMPASS_ID_MMC314X:
+ SetupCompassMMCCalibration(platformId);
+ break;
+ case COMPASS_ID_HSCDTD002B:
+ SetupCompassHSCDTD002BCalibration(platformId);
+ break;
+ case COMPASS_ID_HSCDTD004A:
+ SetupCompassHSCDTD004ACalibration(platformId);
+ break;
+ case ID_INVALID:
+ break;
+ default:
+ if (INV_ERROR_INVALID_PARAMETER) {
+ LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+ break;
+ }
+
+ if (platformId == PLATFORM_ID_MSB_V2_MANTIS ||
+ platformId == PLATFORM_ID_MANTIS_MSB ||
+ platformId == PLATFORM_ID_MANTIS_USB_DONGLE ||
+ platformId == PLATFORM_ID_MANTIS_PROTOTYPE ||
+ platformId == PLATFORM_ID_DRAGON_PROTOTYPE) {
+ switch (compassId) {
+ case ID_INVALID:
+ g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID;
+ break;
+ case COMPASS_ID_AK8975:
+ case COMPASS_ID_AMI306:
+ g_pdata_slave_compass.bus = EXT_SLAVE_BUS_SECONDARY;
+ break;
+ default:
+ g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY;
+ };
+ } else {
+ g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY;
+ }
+
+#ifndef WIN32
+ if (compassId != ID_INVALID)
+ Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation);
+#endif
+
+ return INV_SUCCESS;
+}
+
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the part was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+#if 0
+inv_error_t SetupPressureCalibration(unsigned short platformId,
+ unsigned short pressureId)
+{
+ inv_error_t result = INV_SUCCESS;
+ /*---- setup the compass ----*/
+ switch(pressureId) {
+ case PRESSURE_ID_BMA085:
+ result = SetupPressureBMA085Calibration(platformId);
+ break;
+ default:
+ if (INV_ERROR_INVALID_PARAMETER) {
+ LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+ };
+
+ return result;
+}
+#endif
+/**
+ * @internal
+ * Sets up the orientation matrix according to how the gyro was
+ * mounted.
+ *
+ * @param platforId Platform identification for mounting information
+ * @return INV_SUCCESS or non-zero error code
+ */
+static inv_error_t SetupGyroCalibration(unsigned short platformId)
+{
+ int position;
+ MPL_LOGV("Calibrating '%s'\n", __func__);
+
+ /* Orientation */
+ switch (platformId) {
+ case PLATFORM_ID_SPIDER_PROTOTYPE:
+ position = 2;
+ break;
+ case PLATFORM_ID_MSB:
+ case PLATFORM_ID_MSB_10AXIS:
+ case PLATFORM_ID_MANTIS_MSB:
+#ifndef WIN32
+ position = 4;
+#else
+ position = 6;
+#endif
+ break;
+ case PLATFORM_ID_DONGLE:
+ case PLATFORM_ID_MANTIS_USB_DONGLE:
+ position = 1;
+ break;
+ case PLATFORM_ID_DRAGON_USB_DONGLE:
+ position = 3;
+ break;
+ case PLATFORM_ID_MANTIS_PROTOTYPE:
+ case PLATFORM_ID_DRAGON_PROTOTYPE:
+ case PLATFORM_ID_ST_6AXIS:
+ case PLATFORM_ID_MSB_V2:
+ case PLATFORM_ID_MSB_V2_MANTIS:
+#ifndef WIN32
+ position = 2;
+#else
+ position = 0;
+#endif
+ break;
+ case PLATFORM_ID_MANTIS_EVB:
+ case PLATFORM_ID_MSB_EVB:
+ position = 0;
+ break;
+ case PLATFORM_ID_MSB_V3:
+ position = 2;
+ break;
+ default:
+ MPL_LOGE("Unsupported platform %d\n", platformId);
+ return INV_ERROR_INVALID_PARAMETER;
+ };
+
+ SetupOrientation(position, g_gyro_orientation);
+
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief Setup the Hw orientation and full scale.
+ * @param platfromId
+ * an user defined Id to distinguish the Hw platform in
+ * use from others.
+ * @param accelId
+ * the accelerometer specific id, as specified in the MPL.
+ * @param compassId
+ * the compass specific id, as specified in the MPL.
+ * @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t SetupPlatform(
+ unsigned short platformId,
+ unsigned short accelId,
+ unsigned short compassId)
+{
+ int result;
+
+ memset(&g_slave_accel, 0, sizeof(g_slave_accel));
+ memset(&g_slave_compass, 0, sizeof(g_slave_compass));
+// memset(&g_slave_pressure, 0, sizeof(g_slave_pressure));
+// memset(&g_pdata, 0, sizeof(g_pdata));
+
+#ifdef LINUX
+ /* On Linux initialize the global platform data with the driver defaults */
+ {
+ void *mpu_handle;
+ int ii;
+
+ struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
+ struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
+ slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+ slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
+ slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
+ //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;
+
+ pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+ pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
+ pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
+ //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;
+
+ MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n");
+ result = inv_serial_open("/dev/mpu",&mpu_handle);
+ if (result) {
+ MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
+ }
+ for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+ if (!slave[ii])
+ continue;
+ slave[ii]->type = ii;
+ result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR,
+ slave[ii]);
+ if (result)
+ result = errno;
+ if(result == INV_ERROR_INVALID_MODULE) {
+ slave[ii] = NULL;
+ result = 0;
+ } else if (result) {
+ LOG_RESULT_LOCATION(result);
+ LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE);
+ return result;
+ }
+ }
+ //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata);
+ if (result) {
+ result = errno;
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+ if (!pdata_slave[ii])
+ continue;
+ pdata_slave[ii]->type = ii;
+ result = ioctl(
+ (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA,
+ pdata_slave[ii]);
+ if (result)
+ result = errno;
+ if (result == INV_ERROR_INVALID_MODULE) {
+ pdata_slave[ii] = NULL;
+ result = 0;
+ } else if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+ if (result) {
+ MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result);
+ }
+ inv_serial_close(mpu_handle);
+ }
+#endif
+
+ result = SetupGyroCalibration(platformId);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ PrintMountingOrientation("Gyroscope", g_gyro_orientation);
+ result = SetupAccelCalibration(platformId, accelId);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation);
+ result = SetupCompassCalibration(platformId, compassId);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation);
+#if 0
+ if (platformId == PLATFORM_ID_MSB_10AXIS) {
+ result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation);
+ }
+#endif
+#ifdef LINUX
+ /* On Linux override the orientation, level shifter etc */
+ {
+ void *mpu_handle;
+ int ii;
+ struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES];
+ struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES];
+ slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+ slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel;
+ slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass;
+ //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure;
+
+ pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL;
+ pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel;
+ pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass;
+ //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure;
+
+ MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n");
+ result = inv_serial_open("/dev/mpu",&mpu_handle);
+ if (result) {
+ MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
+ }
+ for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+ if (!slave[ii])
+ continue;
+ slave[ii]->type = ii;
+ result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
+ slave[ii]);
+ if (result)
+ result = errno;
+ if (result == INV_ERROR_INVALID_MODULE) {
+ slave[ii] = NULL;
+ result = 0;
+ } else if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+ //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata);
+ if (result) {
+ result = errno;
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+ if (!pdata_slave[ii])
+ continue;
+ pdata_slave[ii]->type = ii;
+ result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA,
+ pdata_slave[ii]);
+ if (result)
+ result = errno;
+ if (result == INV_ERROR_INVALID_MODULE) {
+ pdata_slave[ii] = NULL;
+ result = 0;
+ } else if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+ if (result) {
+ MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result);
+ }
+ inv_serial_close(mpu_handle);
+ }
+#endif
+ return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
+
+
diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.h b/libsensors_iio/software/simple_apps/common/mlsetup.h
new file mode 100644
index 0000000..06fa9f4
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/mlsetup.h
@@ -0,0 +1,52 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id: mlsetup.h 6101 2011-09-29 00:30:33Z kkatingari $
+ *
+ *******************************************************************************/
+
+#ifndef MLSETUP_H
+#define MLSETUP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "linux/mpu.h"
+#include "mltypes.h"
+
+ enum mpu_platform_id {
+ PLATFORM_ID_INVALID = ID_INVALID, // 0
+ PLATFORM_ID_MSB, // (0x0001) MSB (Multi sensors board)
+ PLATFORM_ID_ST_6AXIS, // (0x0002) 6 Axis with ST accelerometer
+ PLATFORM_ID_DONGLE, // (0x0003) 9 Axis USB dongle with
+ PLATFORM_ID_MANTIS_PROTOTYPE, // (0x0004) Mantis prototype board
+ PLATFORM_ID_MANTIS_MSB, // (0x0005) MSB with Mantis
+ PLATFORM_ID_MANTIS_USB_DONGLE,// (0x0006) Mantis and AKM on USB dongle.
+ PLATFORM_ID_MSB_10AXIS, // (0x0007) MSB with pressure sensor
+ PLATFORM_ID_DRAGON_PROTOTYPE, // (0x0008) Dragon prototype board
+ PLATFORM_ID_MSB_V2, // (0x0009) Version 2 MSB
+ PLATFORM_ID_MSB_V2_MANTIS, // (0x000A) Version 2 MSB with mantis
+ PLATFORM_ID_MANTIS_EVB, // (0x000B) Mantis EVB (shipped to cust.)
+ PLATFORM_ID_DRAGON_USB_DONGLE,// (0x000C) Dragon USB Dongle with Mantis Rev C
+ PLATFORM_ID_MSB_EVB, // (0X000D) MSB with 3050.
+ PLATFORM_ID_SPIDER_PROTOTYPE,
+ PLATFORM_ID_MSB_V3,
+
+ NUM_PLATFORM_IDS
+ };
+ // Main entry APIs
+inv_error_t SetupPlatform(unsigned short platformId,
+ unsigned short accelSelection,
+ unsigned short compassSelection);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* MLSETUP_H */
diff --git a/libsensors_iio/software/simple_apps/common/slave.h b/libsensors_iio/software/simple_apps/common/slave.h
new file mode 100644
index 0000000..7b40a8c
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/common/slave.h
@@ -0,0 +1,176 @@
+/*
+ $License:
+ Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: slave.h 5732 2011-07-07 01:11:34Z vbhatt $
+ *
+ *******************************************************************************/
+
+#ifndef SLAVE_H
+#define SLAVE_H
+
+/**
+ * @addtogroup SLAVEDL
+ *
+ * @{
+ * @file slave.h
+ * @brief Top level descriptions for Accelerometer support
+ *
+ */
+
+#include "mltypes.h"
+#include "linux/mpu.h"
+
+ /* ------------ */
+ /* - Defines. - */
+ /* ------------ */
+
+/*--- default accel support - selection ---*/
+#define ACCEL_ST_LIS331 0
+#define ACCEL_KIONIX_KXTF9 1
+#define ACCEL_BOSCH 0
+#define ACCEL_ADI 0
+
+#define ACCEL_SLAVEADDR_INVALID 0x00
+
+#define ACCEL_SLAVEADDR_LIS331 0x18
+#define ACCEL_SLAVEADDR_LSM303 0x18
+#define ACCEL_SLAVEADDR_LIS3DH 0x18
+#define ACCEL_SLAVEADDR_KXSD9 0x18
+#define ACCEL_SLAVEADDR_KXTF9 0x0F
+#define ACCEL_SLAVEADDR_BMA150 0x38
+#define ACCEL_SLAVEADDR_BMA222 0x08
+#define ACCEL_SLAVEADDR_BMA250 0x18
+#define ACCEL_SLAVEADDR_ADXL34X 0x53
+#define ACCEL_SLAVEADDR_ADXL34X_ALT 0x1D /* alternative addr */
+#define ACCEL_SLAVEADDR_MMA8450 0x1C
+#define ACCEL_SLAVEADDR_MMA845X 0x1C
+
+#define ACCEL_SLAVEADDR_INVENSENSE 0x68
+/*
+ Define default accelerometer to use if no selection is made
+*/
+#if ACCEL_ST_LIS331
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LIS331
+#define DEFAULT_ACCEL_ID ACCEL_ID_LIS331
+#endif
+
+#if ACCEL_ST_LSM303
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LSM303
+#define DEFAULT_ACCEL_ID ACCEL_ID_LSM303DLX
+#endif
+
+#if ACCEL_KIONIX_KXSD9
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXSD9
+#define DEFAULT_ACCEL_ID ACCEL_ID_KXSD9
+#endif
+
+#if ACCEL_KIONIX_KXTF9
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXTF9
+#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9
+#endif
+
+#if ACCEL_BOSCH
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA150
+#define DEFAULT_ACCEL_ID ACCEL_ID_BMA150
+#endif
+
+#if ACCEL_BMA222
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA222
+#define DEFAULT_ACCEL_ID ACCEL_ID_BMA222
+#endif
+
+#if ACCEL_BOSCH
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA250
+#define DEFAULT_ACCEL_ID ACCEL_ID_BMA250
+#endif
+
+#if ACCEL_ADI
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_ADXL34X
+#define DEFAULT_ACCEL_ID ACCEL_ID_ADXL34X
+#endif
+
+#if ACCEL_MMA8450
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA8450
+#define DEFAULT_ACCEL_ID ACCEL_ID_MMA8450
+#endif
+
+#if ACCEL_MMA845X
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA845X
+#define DEFAULT_ACCEL_ID ACCEL_ID_MMA845X
+#endif
+
+/*--- if no default accelerometer was selected ---*/
+#ifndef DEFAULT_ACCEL_SLAVEADDR
+#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_INVALID
+#endif
+
+#define USE_COMPASS_AICHI 0
+#define USE_COMPASS_AKM 0
+#define USE_COMPASS_YAS529 0
+#define USE_COMPASS_YAS530 0
+#define USE_COMPASS_HMC5883 0
+#define USE_COMPASS_MMC314X 0
+#define USE_COMPASS_HSCDTD002B 0
+#define USE_COMPASS_HSCDTD004A 0
+
+#define COMPASS_SLAVEADDR_INVALID 0x00
+#define COMPASS_SLAVEADDR_AKM_BASE 0x0C
+#define COMPASS_SLAVEADDR_AKM 0x0E
+#define COMPASS_SLAVEADDR_AMI304 0x0E
+#define COMPASS_SLAVEADDR_AMI305 0x0F /*Slave address for AMI 305/306*/
+#define COMPASS_SLAVEADDR_AMI306 0x0E /*Slave address for AMI 305/306*/
+#define COMPASS_SLAVEADDR_YAS529 0x2E
+#define COMPASS_SLAVEADDR_YAS530 0x2E
+#define COMPASS_SLAVEADDR_HMC5883 0x1E
+#define COMPASS_SLAVEADDR_MMC314X 0x30
+#define COMPASS_SLAVEADDR_HSCDTD00XX 0x0C
+
+/*
+ Define default compass to use if no selection is made
+*/
+ #if USE_COMPASS_AKM
+ #define DEFAULT_COMPASS_TYPE COMPASS_ID_AK8975
+ #endif
+
+ #if USE_COMPASS_AICHI
+ #define DEFAULT_COMPASS_TYPE COMPASS_ID_AMI30X
+ #endif
+
+ #if USE_COMPASS_YAS529
+ #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS529
+ #endif
+
+ #if USE_COMPASS_YAS530
+ #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS530
+ #endif
+
+ #if USE_COMPASS_HMC5883
+ #define DEFAULT_COMPASS_TYPE COMPASS_ID_HMC5883
+ #endif
+
+#if USE_COMPASS_MMC314X
+#define DEFAULT_COMPASS_TYPE COMPASS_ID_MMC314X
+#endif
+
+#if USE_COMPASS_HSCDTD002B
+#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD002B
+#endif
+
+#if USE_COMPASS_HSCDTD004A
+#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD004A
+#endif
+
+#ifndef DEFAULT_COMPASS_TYPE
+#define DEFAULT_COMPASS_TYPE ID_INVALID
+#endif
+
+
+#endif // SLAVE_H
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
new file mode 100644
index 0000000..228abf7
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared
Binary files differ
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
similarity index 77%
copy from libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
copy to libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
index 7655e4d..b1d881c 100644
--- a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
+++ b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk
@@ -1,4 +1,4 @@
-EXEC = inv_gesture_test$(SHARED_APP_SUFFIX)
+EXEC = consoledmp$(SHARED_APP_SUFFIX)
MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
@@ -8,10 +8,12 @@
OBJFOLDER = $(CURDIR)/obj
-INV_ROOT = ../../../../..
+INV_ROOT = ../../../../../..
APP_DIR = $(CURDIR)/../..
MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
MPL_DIR = $(INV_ROOT)/software/core/mpl
+HAL_DIR = $(INV_ROOT)/software/core/HAL
include $(INV_ROOT)/software/build/android/common.mk
@@ -47,8 +49,20 @@
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 += $(ANDROID_LINK_EXECUTABLE)
+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
LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
@@ -73,7 +87,7 @@
$(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)
+ $(LINK) $(PRE_LFLAGS) $(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/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
new file mode 100644
index 0000000..b01fdfb
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk
@@ -0,0 +1,23 @@
+#### filelist.mk for console_test ####
+
+# helper headers
+HEADERS := $(COMMON_DIR)/external_hardware.h
+HEADERS += $(COMMON_DIR)/fopenCMake.h
+HEADERS += $(COMMON_DIR)/helper.h
+HEADERS += $(COMMON_DIR)/mlerrorcode.h
+HEADERS += $(COMMON_DIR)/mlsetup.h
+HEADERS += $(COMMON_DIR)/slave.h
+
+HEADERS += $(HAL_DIR)/include/mlos.h
+HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
+
+# sources
+SOURCES := $(APP_DIR)/console_test.c
+
+# helper sources
+SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c
+SOURCES += $(HAL_DIR)/linux/mlos_linux.c
+
+INV_SOURCES += $(SOURCES)
+
+VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c
new file mode 100644
index 0000000..e15b20d
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/console/linux/console_test.c
@@ -0,0 +1,742 @@
+/*******************************************************************************
+ * $Id: $
+ ******************************************************************************/
+
+/*******************************************************************************
+ *
+ * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved.
+ *
+ ******************************************************************************/
+//#include <linux/conio.h>
+//#include <fcntl.h>
+//#include <termios.h>
+//#include <unistd.h>
+
+//#if 0
+#include <stdio.h>
+#include <time.h>
+#include <sys/select.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <linux/input.h>
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "console_test"
+
+#include "mlos.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "inv_sysfs_utils.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+/*#else
+#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 "inv_sysfs_utils.h"
+#include "invensense.h"
+#include "invensense_adv.h"
+#include "ml_stored_data.h"
+#include "ml_math_func.h"
+#include "ml_load_dmp.h"
+#include "log.h"
+#endif*/
+
+/* TODO: add devices as needed. */
+#define ITG3500 (0)
+#define MPU6050 (1)
+#define BMA250 (2)
+#define NUM_DEVICES (ITG3500 + MPU6050 + BMA250)
+
+#define DEVICE MPU6050
+#define DMP_IMAGE dmp_firmware_200_latest
+#define SIX_AXES 6
+#define NINE_AXES 9
+
+#if 0
+struct input_event {
+ struct timeval time;
+ __u16 type;
+ __u16 code;
+ __s32 value;
+};
+#endif
+
+/* TODO: Add paths to other attributes.
+ * TODO: Input device paths depend on the module installation order.
+ */
+const struct inv_sysfs_names_s filenames[NUM_DEVICES] = {
+ { /* ITG3500 */
+ .buffer = "/dev/input/event0",
+ .enable = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable",
+ .raw_data = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro",
+ .temperature = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature",
+ .fifo_rate = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate",
+ .power_state = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state",
+ .fsr = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR",
+ .lpf = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf",
+ .scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale",
+ .temp_scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale",
+ .temp_offset = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset",
+ .self_test = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test",
+ .accel_en = NULL,
+ .accel_fifo_en = NULL,
+ .accel_fs = NULL,
+ .clock_source = NULL,
+ .early_suspend_en = NULL,
+ .firmware_loaded = NULL,
+ .gyro_en = NULL,
+ .gyro_fifo_en = NULL,
+ .key = NULL,
+ .raw_accel = NULL,
+ .reg_dump = NULL,
+ .tap_on = NULL,
+ .dmp_firmware = NULL
+ },
+
+ { /* MPU6050 */
+ .buffer = "/dev/input/event0",
+ .enable = "/sys/class/invensense/mpu/enable",
+ .raw_data = "/sys/class/invensense/mpu/raw_gyro",
+ .temperature = "/sys/class/invensense/mpu/temperature",
+ .fifo_rate = "/sys/class/invensense/mpu/fifo_rate",
+ .power_state = "/sys/class/invensense/mpu/power_state",
+ .fsr = "/sys/class/invensense/mpu/FSR",
+ .lpf = "/sys/class/invensense/mpu/lpf",
+ .scale = "/sys/class/invensense/mpu/gyro_scale",
+ .temp_scale = "/sys/class/invensense/mpu/temp_scale",
+ .temp_offset = "/sys/class/invensense/mpu/temp_offset",
+ .self_test = "/sys/class/invensense/mpu/self_test",
+ .accel_en = "/sys/class/invensense/mpu/accl_enable",
+ .accel_fifo_en = "/sys/class/invensense/mpu/accl_fifo_enable",
+ .accel_fs = "/sys/class/invensense/mpu/accl_fs",
+ .clock_source = "/sys/class/invensense/mpu/clock_source",
+ .early_suspend_en = "/sys/class/invensense/mpu/early_suspend_enable",
+ .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded",
+ .gyro_en = "/sys/class/invensense/mpu/gyro_enable",
+ .gyro_fifo_en = "/sys/class/invensense/mpu/gyro_fifo_enable",
+ .key = "/sys/class/invensense/mpu/key",
+ .raw_accel = "/sys/class/invensense/mpu/raw_accl",
+ .reg_dump = "/sys/class/invensense/mpu/reg_dump",
+ .tap_on = "/sys/class/invensense/mpu/tap_on",
+ .dmp_firmware = "/sys/class/invensense/mpu/dmp_firmware"
+ },
+
+ { /* BMA250 */
+ .buffer = "/dev/input/input/event1",
+ .enable = "/sys/devices/virtual/input/input1/enable",
+ .raw_data = "/sys/devices/virtual/input/input1/value",
+ .temperature = NULL,
+ .fifo_rate = NULL,
+ .power_state = NULL,
+ .fsr = NULL,
+ .lpf = NULL,
+ .scale = NULL,
+ .temp_scale = NULL,
+ .temp_offset = NULL,
+ .self_test = NULL,
+ .accel_en = NULL,
+ .accel_fifo_en = NULL,
+ .accel_fs = NULL,
+ .clock_source = NULL,
+ .early_suspend_en = NULL,
+ .firmware_loaded = NULL,
+ .gyro_en = NULL,
+ .gyro_fifo_en = NULL,
+ .key = NULL,
+ .raw_accel = NULL,
+ .reg_dump = NULL,
+ .tap_on = NULL,
+ .dmp_firmware = NULL
+ }
+};
+
+static void (*s_func_cb) (void);
+
+int inv_read_data(char *names, char *data)
+{
+ char str[8];
+ int count;
+ short s_data;
+
+ count = inv_sysfs_read((char*)names, sizeof(str), str);
+ if (count < 0)
+ return count;
+ count = sscanf(str, "%hd", &s_data);
+ *data = s_data;
+ if (count < 1)
+ return -EAGAIN;
+ return count;
+
+}
+
+void fifoCB(void)
+{
+ if (1) {
+ float gyro[3];
+ float accel[3];
+ float orient[3];
+ float rv[3];
+
+ int8_t accuracy;
+ inv_time_t timestamp;
+
+ printf("/*************************************************\n");
+ inv_get_sensor_type_gyroscope(gyro, &accuracy, ×tamp);
+ printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n",
+ gyro[0],
+ gyro[1],
+ gyro[2],
+ accuracy,
+ timestamp);
+
+ inv_get_sensor_type_accelerometer(accel, &accuracy, ×tamp);
+ printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n",
+ accel[0],
+ accel[1],
+ accel[2],
+ accuracy,
+ timestamp);
+
+ inv_get_sensor_type_rotation_vector(rv, &accuracy, ×tamp);
+ printf("RV %7.3f %7.3f %7.3f %5d %9lld\n",
+ rv[0],rv[1],rv[2],accuracy,timestamp);
+
+ inv_get_sensor_type_orientation(orient, &accuracy, ×tamp);
+ printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n",
+ orient[0],orient[1],orient[2],accuracy,timestamp);
+ printf("/*************************************************\n");
+ }
+}
+
+unsigned short orient;
+signed char g_gyro_orientation[9] = {1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1};
+
+signed char g_accel_orientation[9] = {-1, 0, 0,
+ 0, -1, 0,
+ 0, 0, 1};
+float scale;
+float range;
+long sens;
+
+
+
+short mTempOffset = 0;
+short mTempScale = 0;
+bool mFirstRead = 1;
+
+/******************* FUNCTIONS *******************************/
+#if 0
+static unsigned short inv_row_2_scale(const signed char *row)
+{
+ unsigned short b;
+
+ if (row[0] > 0)
+ b = 0;
+ else if (row[0] < 0)
+ b = 4;
+ else if (row[1] > 0)
+ b = 1;
+ else if (row[1] < 0)
+ b = 5;
+ else if (row[2] > 0)
+ b = 2;
+ else if (row[2] < 0)
+ b = 6;
+ else
+ b = 7; // error
+ return b;
+}
+#endif
+
+inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void))
+{
+ s_func_cb = func_cb;
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief Keyboard hit function.
+ */
+int kbhit(void)
+{
+#if 1
+ struct timeval tv;
+ fd_set read_fd;
+
+ tv.tv_sec=0;
+ tv.tv_usec=0;
+ FD_ZERO(&read_fd);
+ FD_SET(0,&read_fd);
+
+ if (select(1, &read_fd, NULL, NULL, &tv) == -1)
+ return 0;
+
+ if (FD_ISSET(0,&read_fd))
+ return 1;
+
+ return 0;
+#else
+ struct timeval tv;
+ fd_set rdfs;
+
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+
+ FD_ZERO(&rdfs);
+ FD_SET (STDIN_FILENO, &rdfs);
+
+ select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);
+ return FD_ISSET(STDIN_FILENO, &rdfs);
+#endif
+}
+
+inv_error_t inv_constructor_default_enable()
+{
+ inv_error_t result;
+
+ result = inv_enable_quaternion();
+ if (result) {
+ if (result == INV_ERROR_NOT_AUTHORIZED) {
+ LOGE("Enable Quaternion failed: not authorized");
+ }
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_motion_no_motion();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_gyro_tc();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_hal_outputs();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = inv_enable_9x_sensor_fusion();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ return result;
+}
+
+int read_attribute_sensor(int fd, char *data, unsigned int size)
+{
+ int count = 0;
+ if (fd >=0) {
+ count = read(fd, data, size);
+ if(count < 0) {
+ MPL_LOGE("read fails with error code=%d", count);
+ }
+ close(fd);
+ }
+ return count;
+}
+
+int inv_read_temperature(long long *data)
+{
+ int count = 0;
+ int fd;
+
+ if(mFirstRead) {
+ char buf[4];
+ fd = open(filenames[ITG3500].temp_scale, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("errors opening tempscale");
+ return -1;
+ }
+
+ memset(buf, 0, sizeof(buf));
+
+ count = read_attribute_sensor(fd, buf, sizeof(buf));
+ if(count < 0) {
+ MPL_LOGE("errors reading temp_scale");
+ return -1;
+ }
+
+ count = sscanf(buf, "%hd", &mTempScale);
+ if(count < 1)
+ return -1;
+ MPL_LOGI("temp scale = %d", mTempScale);
+
+ fd = open(filenames[ITG3500].temp_offset, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("errors opening tempoffset");
+ return -1;
+ }
+
+ memset(buf, 0, sizeof(buf));
+
+ count = read_attribute_sensor(fd, buf, sizeof(buf));
+ if(count < 0) {
+ MPL_LOGE("errors reading temp_offset");
+ return -1;
+ }
+
+ count = sscanf(buf, "%hd", &mTempOffset);
+ if(count < 1)
+ return -1;
+ MPL_LOGI("temp offset = %d", mTempOffset);
+
+ mFirstRead = false;
+ }
+
+ char raw_buf[25];
+ short raw;
+ long long timestamp;
+ fd = open(filenames[ITG3500].temperature, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("errors opening temperature");
+ return -1;
+ }
+
+ memset(raw_buf, 0, sizeof(raw_buf));
+
+ count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
+ if(count < 0) {
+ MPL_LOGE("errors reading temperature");
+ return -1;
+ }
+ count = sscanf(raw_buf, "%hd%lld", &raw, ×tamp);
+ if(count < -1)
+ return -1;
+ MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp);
+ MPL_LOGI("temperature offset = %d", mTempOffset);
+ MPL_LOGI("temperature scale = %d", mTempScale);
+ int adjuster = 35 + ((raw-mTempOffset)/mTempScale);
+ MPL_LOGI("pre-scaled temperature = %d", adjuster);
+ MPL_LOGI("adjusted temperature = %d", adjuster*65536);
+ //data[0] = adjuster * 65536;
+ data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f;
+ data[1] = timestamp;
+ return 0;
+}
+
+int self_test(void)
+{
+ int err = 0;
+ char str[50];
+ char x[9], y[9], z[9];
+ char pass[2];
+ int fd;
+
+ fd = open((char*)filenames[DEVICE].self_test, O_RDONLY);
+ if(fd < 0) {
+ return fd;
+ }
+ memset(str, 0, sizeof(str));
+ err = read_attribute_sensor(fd, str, sizeof(str));
+ if(err < 0) {
+ return err;
+ }
+ MPL_LOGI("self_test result: %s", str);
+ printf("Self test result: %s ", str);
+ err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass);
+ if(err < 1) {
+ return err;
+ }
+ MPL_LOGI("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+ //printf("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z));
+ if (atoi(pass)) {
+ MPL_LOGI("Result : PASS (1)");
+ printf("----> PASS (1)\n");
+ } else {
+ MPL_LOGI("Result : FAIL (0)");
+ printf("----> FAIL (0)\n");
+ }
+ return err;
+}
+
+/*******************************************************************************
+ ******************************* MAIN ******************************************
+ ******************************************************************************/
+
+/**
+ * @brief Main function
+ */
+int main(int argc, char *argv[])
+{
+ int key = 0;
+ int ready;
+ long accel[3];
+ short gyro[3];
+ long long timestamp = 0;
+ inv_error_t result;
+
+ char data;
+ unsigned char i;
+ int fd, bytes_read;
+ struct pollfd pfd;
+ unsigned long long time_stamp;
+ unsigned int time_H;
+ struct input_event ev[100];
+#ifdef INV_PLAYBACK_DBG
+ int logging = false;
+ FILE *logfile = NULL;
+#endif
+
+ result = inv_init_mpl();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ // Master Enabling. This also turns on power_state
+ if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0)
+ printf("ERR- Failed to enable event generation\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].enable, &data);
+ printf("Event enable= %d\n", data);
+ }
+
+ // Power ON - No need after master enable above but do it anyway
+ if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0)
+ printf("ERR- Failed to set power state=1\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].power_state, &data);
+ printf("Power state: %d\n", data);
+ }
+
+ // Turn on tap
+ if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) {
+ printf("ERR- Failed to enable Tap On\n");
+ }
+ else {
+ inv_read_data((char *)filenames[DEVICE].tap_on, &data);
+ printf("Tap-on: %d\n", data);
+ }
+
+ // Program DMP code. No longer required to enable tap-on first
+ if ((result =
+ inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) {
+ printf("ERR- Failed to initiate DMP re-programming %d\n",result);
+ } else {
+ if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) {
+ printf("ERR- Failed file open to write DMP\n");
+ close(fd);
+ exit(0);
+ } else {
+ // Program 200Hz version
+ //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE));
+ //printf("Downloaded %d byte(s) to DMP\n", result);
+ result = inv_load_dmp(fd);
+ //LOG_RESULT_LOCATION(result);
+ close(fd);
+ }
+ }
+
+ // Query DMP running. For now check by 'firmware_loaded' status
+ if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) {
+ printf("ERR- Failed to read 'firmware_loaded'\n");
+ } else {
+ printf("Firmware Loaded/ DMP running: %d\n", data);
+ }
+
+ inv_set_fifo_processed_callback(fifoCB);
+ result = inv_constructor_default_enable();
+ result = inv_start_mpl();
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ } else {
+ printf ("MPL started\n");
+ }
+
+ /* Gyro Setup */
+ orient = inv_orientation_matrix_to_scalar(g_gyro_orientation);
+ inv_set_gyro_orientation_and_scale(orient,2000L<<15);
+
+ /* Accel Setup */
+ orient = inv_orientation_matrix_to_scalar(g_accel_orientation);
+ /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data.
+ * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits.
+ * 2 * 1L<<15 * 1<<6 == 1LL<<22
+ */
+ inv_set_accel_orientation_and_scale(orient, 1LL<<22);
+
+ // Enable Gyro
+ if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0)
+ printf("ERR- Failed to enable Gyro\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].gyro_en, &data);
+ printf("Gyro enable: %d\n", data);
+ }
+
+ // Enable Accel
+ if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0)
+ printf("ERR- Failed to enable Accel\n");
+ else {
+ inv_read_data((char *)filenames[DEVICE].accel_en, &data);
+ printf("Accel enable: %d\n", data);
+ }
+
+ // polling for data
+ fd = open(filenames[DEVICE].buffer, O_RDONLY);
+ if(fd < 0) {
+ MPL_LOGE("Cannot open device event buffer");
+ }
+
+ pfd.fd = fd;
+ pfd.events = POLLIN;
+
+ while (1) {
+
+ result = kbhit();
+ if (result) {
+ key = getchar();
+ } else {
+ key = 0;
+ }
+ if (key == 'l') {
+ MPL_LOGI(" 'l' - load calibration file");
+ inv_load_calibration();
+ }
+ if (key == 't') {
+ MPL_LOGI(" 't' - self test");
+ self_test();
+ }
+ if (key == 'q') {
+ MPL_LOGI(" 'q' - store calibration file");
+ inv_store_calibration();
+ break;
+ }
+#ifdef INV_PLAYBACK_DBG
+ if (key == 's') {
+ if (!logging) {
+ MPL_LOGI(" 's' - toggle logging on");
+ logfile = fopen("/data/playback.bin", "wb");
+ if (logfile) {
+ inv_turn_on_data_logging(logfile);
+ logging = true;
+ } else {
+ MPL_LOGI("Error : "
+ "cannot open log file '/data/playback.bin'");
+ }
+ } else {
+ MPL_LOGI(" 's' - toggle logging off");
+ inv_turn_off_data_logging();
+ fclose(logfile);
+ logging = false;
+ }
+ break;
+ }
+#endif
+
+ ready = poll(&pfd, 1, 100);
+ if (ready) {
+ bytes_read = read_attribute_sensor(fd, (char *)ev,
+ sizeof(struct input_event) * SIX_AXES);
+ //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES);
+ if (bytes_read > 0) {
+ int executed;
+
+ for (i = 0; i < bytes_read / sizeof(struct input_event); i++) {
+ if (ev[i].type == EV_REL) {
+ switch (ev[i].code) {
+ case REL_X:
+ printf("REL_X\n");
+ gyro[0]= ev[i].value; //Gyro X
+ printf("Gyro X:%5d ", gyro[0]);
+ break;
+ case REL_Y:
+ printf("REL_Y\n");
+ gyro[1]= ev[i].value; //Gyro Y
+ printf("Gyro Y:%5d ", gyro[1]);
+ break;
+ case REL_Z:
+ printf("REL_Z\n");
+ gyro[2]= ev[i].value; //Gyro Z
+ printf("Gyro Z:%5d ", gyro[2]);
+ break;
+ case REL_RX:
+ printf("REL_RX\n");
+ accel[0]= ev[i].value; //Accel X
+ printf("Accl X:%5ld ", accel[0]);
+ break;
+ case REL_RY:
+ printf("REL_RY\n");
+ accel[1]= ev[i].value; //Accel Y
+ printf("Accl Y:%5ld ", accel[1]);
+ break;
+ case REL_RZ:
+ printf("REL_RZ\n");
+ accel[2]= ev[i].value; //Accel Z
+ printf("Accl Z:%5ld ", accel[2]);
+ break;
+ case REL_MISC:
+ time_H= ev[i].value;
+ break;
+ case REL_WHEEL:
+ time_stamp = ((unsigned long long)(time_H) << 32) +
+ (unsigned int)ev[i].value;
+ break;
+ default:
+ printf("ERR- Un-recognized event code: %5d ", ev[i].code);
+ break;
+ }
+ } else {
+#if 0
+ clock_gettime(CLOCK_MONOTONIC, &timer);
+ curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL;
+ printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL);
+#endif
+ }
+ }
+
+ // build & process gyro + accel data
+ result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ } else if ((result = inv_build_accel(accel, 0,
+ (inv_time_t)timestamp,
+ &executed))) {
+ LOG_RESULT_LOCATION(result);
+ }
+ if (executed) {
+ printf("Exec on data Ok\n");
+ s_func_cb();
+ }
+
+ } else {
+ //printf ("ERR- No data!\n");
+ }
+
+ } else { MPL_LOGV("Device not ready"); }
+ }
+ close(fd);
+
+#ifdef INV_PLAYBACK_DBG
+ if (logging) {
+ inv_turn_off_data_logging();
+ fclose(logfile);
+ }
+#endif
+
+ return 0;
+}
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
deleted file mode 100644
index ede049d..0000000
--- a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared
+++ /dev/null
Binary files differ
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
deleted file mode 100644
index 75d93cf..0000000
--- a/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk
+++ /dev/null
@@ -1,11 +0,0 @@
-#### 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
deleted file mode 100644
index d38d478..0000000
--- a/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c
+++ /dev/null
@@ -1,535 +0,0 @@
-/**
- * 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/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
new file mode 100644
index 0000000..5d52b21
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared
Binary files differ
diff --git a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
similarity index 78%
rename from libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
rename to libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
index 7655e4d..7f6cc43 100644
--- a/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk
+++ b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk
@@ -1,4 +1,4 @@
-EXEC = inv_gesture_test$(SHARED_APP_SUFFIX)
+EXEC = input_gyro$(SHARED_APP_SUFFIX)
MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
@@ -11,7 +11,9 @@
INV_ROOT = ../../../../..
APP_DIR = $(CURDIR)/../..
MLLITE_DIR = $(INV_ROOT)/software/core/mllite
+COMMON_DIR = $(INV_ROOT)/software/simple_apps/common
MPL_DIR = $(INV_ROOT)/software/core/mpl
+HAL_DIR = $(INV_ROOT)/software/core/HAL
include $(INV_ROOT)/software/build/android/common.mk
@@ -47,8 +49,20 @@
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 += $(ANDROID_LINK_EXECUTABLE)
+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
LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
@@ -73,7 +87,7 @@
$(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)
+ $(LINK) $(PRE_LFLAGS) $(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/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
new file mode 100644
index 0000000..0936212
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk
@@ -0,0 +1,13 @@
+#### filelist.mk for input_gyro ####
+
+# helper headers
+HEADERS := $(MPL_DIR)/authenticate.h
+#HEADERS +=
+
+# sources
+SOURCES := $(APP_DIR)/test_input_gyro.c
+
+INV_SOURCES += $(SOURCES)
+
+#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
+VPATH += $(APP_DIR)
diff --git a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
new file mode 100644
index 0000000..6fa9aab
--- /dev/null
+++ b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c
@@ -0,0 +1,485 @@
+/*
+ * input interface testing
+ */
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/input.h>
+#include <linux/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "linux/ml_sysfs_helper.h"
+#include "authenticate.h"
+#include "ml_load_dmp.h"
+
+#if 0
+struct input_event {
+ struct timeval time;
+ __u16 type;
+ __u16 code;
+ __s32 value;
+};
+#endif
+
+void HandleOrient(int orient)
+{
+ if (orient & 0x01)
+ printf("INV_X_UP\n");
+ if (orient & 0x02)
+ printf("INV_X_DOWN\n");
+ if (orient & 0x04)
+ printf("INV_Y_UP\n");
+ if (orient & 0x08)
+ printf("INV_Y_DOWN\n");
+ if (orient & 0x10)
+ printf("INV_Z_UP\n");
+ if (orient & 0x20)
+ printf("INV_Z_DOWN\n");
+ if (orient & 0x40)
+ printf("INV_ORIENTATION_FLIP\n");
+}
+
+void HandleTap(int tap)
+{
+ int tap_dir = tap/8;
+ int tap_num = tap%8 + 1;
+
+ switch (tap_dir) {
+ case 1:
+ printf("INV_TAP_AXIS_X_POS\n");
+ break;
+ case 2:
+ printf("INV_TAP_AXIS_X_NEG\n");
+ break;
+ case 3:
+ printf("INV_TAP_AXIS_Y_POS\n");
+ break;
+ case 4:
+ printf("INV_TAP_AXIS_Y_NEG\n");
+ break;
+ case 5:
+ printf("INV_TAP_AXIS_Z_POS\n");
+ break;
+ case 6:
+ printf("INV_TAP_AXIS_Z_NEG\n");
+ break;
+ default:
+ break;
+ }
+ printf("Tap number: %d\n", tap_num);
+}
+
+static void read_compass(int event_number)
+{
+ int ev_size, ret_byte, ii;
+ int fd = -1;
+ struct input_event ev[10];
+ char name[64];
+ char file_name[64];
+ unsigned int RX;
+ unsigned long long time0, time1, time2;
+ struct timespec tsp;
+ ev_size = sizeof(struct input_event);
+ sprintf(file_name, "/dev/input/event%d", event_number);
+ if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+ printf("fail to open compass\n");
+ return;
+ }
+
+ /* NOTE: Use this to pass device name to HAL. */
+ ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+ printf ("Reading From : (%s)\n", name);
+ while (1) {
+ clock_gettime(CLOCK_MONOTONIC, &tsp);
+ /*read compass data here */
+ if(fd > 0){
+ ret_byte = read(fd, ev, ev_size);
+ } else {
+ ret_byte = -1;
+ }
+ time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+ if (ret_byte < 0)
+ continue;
+ for (ii = 0; ii < ret_byte/ev_size; ii++) {
+ if(EV_REL != ev[ii].type) {
+ time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+ printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0));
+ continue;
+ }
+ switch (ev[ii].code) {
+ case REL_X:
+ printf("CX:%5d ", ev[ii].value);
+ break;
+ case REL_Y:
+ printf("CY:%5d ", ev[ii].value);
+ break;
+ case REL_Z:
+ printf("CZ:%5d ", ev[ii].value);
+ break;
+ case REL_MISC:
+ RX = ev[ii].value;
+ break;
+ case REL_WHEEL:
+ time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+ time1 = time1/1000000;
+ printf("time1: %lld ", time1);
+ break;
+ default:
+ printf("GES?: %5d ", ev[ii].code);
+ break;
+ }
+ }
+ }
+ close(fd);
+}
+
+static void read_gesture(int num)
+{
+ int ev_size, ret_byte, ii;
+ int fd = -1;
+ struct input_event ev[10];
+ char name[64];
+ char file_name[64];
+ unsigned long long time;
+ struct timespec tsp;
+ ev_size = sizeof(struct input_event);
+ sprintf(file_name, "/dev/input/event%d", num);
+ MPL_LOGI("%s\n", file_name);
+ if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+ printf("fail to open gusture.\n");
+ return;
+ }
+
+ /* NOTE: Use this to pass device name to HAL. */
+ ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+ printf ("Reading From : (%s)\n", name);
+ while(1){
+ clock_gettime(CLOCK_MONOTONIC, &tsp);
+ if(fd > 0){
+ ret_byte = read(fd, ev, ev_size);
+ } else {
+ ret_byte = -1;
+ }
+ time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL;
+ //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3);
+ if (ret_byte < 0)
+ continue;
+ for (ii = 0; ii < ret_byte/ev_size; ii++) {
+ if(EV_REL != ev[ii].type) {
+ time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL;
+ printf("mono=%lld\n", time);
+ continue;
+ }
+ switch (ev[ii].code) {
+ case REL_RX:
+ printf("GESX:%5x\n", ev[ii].value);
+ HandleTap(ev[ii].value);
+ break;
+ case REL_RY:
+ printf("GESY:%5x\n", ev[ii].value);
+ HandleOrient(ev[ii].value);
+ break;
+ case REL_RZ:
+ printf("FLICK:%5x\n", ev[ii].value);
+ break;
+ default:
+ printf("?: %5d ", ev[ii].code);
+ break;
+ }
+ }
+ }
+}
+
+static void read_gyro_accel(int num)
+{
+ int ev_size, ret_byte, ii;
+ int fd = -1;
+ unsigned int RX;
+ struct input_event ev[10];
+ char name[64];
+ char file_name[64];
+ unsigned long long time0, time1, time2;
+ struct timespec tsp;
+ ev_size = sizeof(struct input_event);
+ sprintf(file_name, "/dev/input/event%d", num);
+ if ((fd = open(file_name, O_RDONLY)) < 0 ) {
+ printf("fail to open gyro/accel\n");
+ return;
+ }
+
+ /* NOTE: Use this to pass device name to HAL. */
+ ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+ printf ("Reading From : (%s)\n", name);
+ while (1){
+ //usleep(20000);
+ ret_byte = read(fd, ev, ev_size);
+ if (ret_byte < 0)
+ continue;
+ //ret_byte = 0;
+
+ for (ii = 0; ii < ret_byte/ev_size; ii++) {
+ if(EV_REL != ev[ii].type) {
+ time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL;
+ printf("T: %lld diff=%d ", time0, (int)(time1 - time0));
+ clock_gettime(CLOCK_MONOTONIC, &tsp);
+ time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL;
+ printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2));
+ continue;
+ }
+ switch (ev[ii].code) {
+ case REL_X:
+ printf("GX:%5d ", ev[ii].value);
+ break;
+ case REL_Y:
+ printf("GY:%5d ", ev[ii].value);
+ break;
+ case REL_Z:
+ printf("GZ:%5d ", ev[ii].value);
+ break;
+ case REL_RX:
+ printf("AX:%5d ", ev[ii].value);
+ break;
+ case REL_RY:
+ printf("AY:%5d ", ev[ii].value);
+ break;
+ case REL_RZ:
+ printf("AZ:%5d ", ev[ii].value);
+ break;
+ case REL_MISC:
+ RX = ev[ii].value;
+ break;
+ case REL_WHEEL:
+ time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value;
+ time1 = time1/1000000;
+ printf("time1: %lld ", time1);
+ break;
+ default:
+ printf("?: %5d ", ev[ii].code);
+ break;
+ }
+ }
+ }
+ close(fd);
+}
+int inv_sysfs_write(char *filename, long data)
+{
+ FILE *fp;
+ int count;
+
+ if (!filename)
+ return -1;
+ fp = fopen(filename, "w");
+ if (!fp)
+ return -errno;
+ count = fprintf(fp, "%ld", data);
+ fclose(fp);
+ return count;
+}
+int inv_sysfs_read(char *filename, long num_bytes, char *data)
+{
+ FILE *fp;
+ int count;
+
+ if (!filename)
+ return -1;
+ fp = fopen(filename, "r");
+ if (!fp)
+ return -errno;
+ count = fread(data, 1, num_bytes, fp);
+ fclose(fp);
+ return count;
+}
+
+void enable_flick(char *p)
+{
+ char sysfs_file[200];
+ printf("flick:%s\n", p);
+ sprintf(sysfs_file, "%s/flick_int_on", p);
+ inv_sysfs_write(sysfs_file, 1);
+ sprintf(sysfs_file, "%s/flick_upper", p);
+ inv_sysfs_write(sysfs_file, 3147790);
+ sprintf(sysfs_file, "%s/flick_lower", p);
+ inv_sysfs_write(sysfs_file, -3147790);
+ sprintf(sysfs_file, "%s/flick_counter", p);
+ inv_sysfs_write(sysfs_file, 50);
+ sprintf(sysfs_file, "%s/flick_message_on", p);
+ inv_sysfs_write(sysfs_file, 0);
+ sprintf(sysfs_file, "%s/flick_axis", p);
+ inv_sysfs_write(sysfs_file, 2);
+}
+
+void setup_dmp(char *sysfs_path)
+{
+ char sysfs_file[200];
+ char firmware_loaded[200], dmp_path[200];
+ char dd[10];
+
+ inv_get_dmpfile(dmp_path);
+ sprintf(sysfs_file, "%s/fifo_rate", sysfs_path);
+ inv_sysfs_write(sysfs_file, 200);
+ sprintf(sysfs_file, "%s/FSR", sysfs_path);
+ inv_sysfs_write(sysfs_file, 2000);
+ sprintf(sysfs_file, "%s/accl_fs", sysfs_path);
+ inv_sysfs_write(sysfs_file, 4);
+ /*
+ sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded");
+ printf("%s\n", firmware_loaded);
+ inv_sysfs_write(firmware_loaded, 0);
+ inv_sysfs_read(firmware_loaded, 1, dd);
+ printf("beforefirmware_loaded=%c\n", dd[0]);
+
+ if ((fd = open(dmp_path, O_WRONLY)) < 0 ) {
+ perror("dmp fail");
+ }
+ inv_load_dmp(fd);
+ close(fd);
+ */
+ inv_sysfs_read(firmware_loaded, 1, dd);
+ printf("firmware_loaded=%c\n", dd[0]);
+}
+void read_pedometer(char *sysfs_path){
+ int steps;
+ char sysfs_file[200];
+ char dd[4];
+ sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path);
+ inv_sysfs_read(sysfs_file, 4, dd);
+ steps = dd[0] << 8 | dd[1];
+ printf("fff=%d\n", steps);
+}
+/* The running sequence:
+ "input_gyro 2 &".
+ This will setup the dmp firmware and let it run on background.
+ tap and flick will work at this time.
+ To see accelerometer data and gyro data.
+ type :
+ "input_gyro ".
+ This will print out gyro data and accelerometer data
+ To see Compass data
+ type:
+ "input_gyro 1" */
+
+int main(int argc, char *argv[])
+{
+ unsigned int RX, i, sel;
+ unsigned char key[16];
+ struct timeval tv;
+ struct timespec tsp0, tsp1, tsp2, tsp3;
+ int event_num;
+ char sysfs_path[200];
+ char chip_name[20];
+ char sysfs_file[200];
+ if (INV_SUCCESS != inv_check_key()) {
+ printf("key check fail\n");
+ exit(0);
+ }else
+ printf("key authenticated\n");
+
+ for(i=0;i<16;i++){
+ key[i] = 0xff;
+ }
+ RX = inv_get_sysfs_key(key);
+ if(RX == INV_SUCCESS){
+ for(i=0;i<16;i++){
+ printf("%d, ", key[i]);
+ }
+ printf("\n");
+ }else{
+ printf("get key failed\n");
+ }
+ memset(sysfs_path, 0, 200);
+ memset(sysfs_file, 0, 200);
+ memset(chip_name, 0, 20);
+ inv_get_sysfs_path(sysfs_path);
+ inv_get_chip_name(chip_name);
+ printf("sysfs path: %s\n", sysfs_path);
+ printf("chip name: %s\n", chip_name);
+ /*set up driver*/
+ sprintf(sysfs_file, "%s/enable", sysfs_path);
+ inv_sysfs_write(sysfs_file, 0);
+ sprintf(sysfs_file, "%s/power_state", sysfs_path);
+ inv_sysfs_write(sysfs_file, 1);
+ if ((getuid ()) != 0)
+ printf ("You are not root! This may not work...\n");
+
+ if(argc ==2 )
+ sel = argv[1][0] - 0x30;
+ else
+ sel = 0;
+ switch(sel){
+ case 0:
+ printf("-------------------------------\n");
+ printf("--- log gyro and accel data ---\n");
+ printf("-------------------------------\n");
+ sprintf(sysfs_file, "%s/enable", sysfs_path);
+ inv_sysfs_write(sysfs_file, 1);
+ if(inv_get_handler_number(chip_name, &event_num) < 0)
+ printf("mpu not installed\n");
+ else
+ read_gyro_accel(event_num);
+ break;
+
+ case 1:
+ printf("------------------------\n");
+ printf("--- log compass data ---\n");
+ printf("------------------------\n");
+ sprintf(sysfs_file, "%s/compass_enable", sysfs_path);
+ inv_sysfs_write(sysfs_file, 1);
+ sprintf(sysfs_file, "%s/enable", sysfs_path);
+ inv_sysfs_write(sysfs_file, 1);
+ if(inv_get_handler_number("INV_COMPASS", &event_num) < 0)
+ printf("compass is not enabled\n");
+ else
+ read_compass(event_num);
+ break;
+
+ case 2:
+ printf("--------------------\n");
+ printf("--- log gestures ---\n");
+ printf("--------------------\n");
+ setup_dmp(sysfs_path);
+ enable_flick(sysfs_path);
+ sprintf(sysfs_file, "%s/tap_on", sysfs_path);
+ inv_sysfs_write(sysfs_file, 1);
+ sprintf(sysfs_file, "%s/enable", sysfs_path);
+ inv_sysfs_write(sysfs_file, 1);
+ if(inv_get_handler_number("INV_DMP", &event_num) < 0)
+ printf("DMP not enabled\n");
+ else
+ read_gesture(event_num);
+ break;
+
+ case 3:
+ printf("-----------------\n");
+ printf("--- pedometer ---\n");
+ printf("-----------------\n");
+ read_pedometer(sysfs_path);
+ break;
+
+ default:
+ printf("error choice\n");
+ break;
+ }
+
+ gettimeofday(&tv, NULL);
+ clock_gettime(CLOCK_MONOTONIC, &tsp1);
+ clock_gettime(CLOCK_REALTIME, &tsp0);
+
+ clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2);
+ clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3);
+ //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC, CLOCK_REALTIME,
+ // CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID);
+ //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec);
+ //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec);
+ //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec);
+ //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec);
+
+ //ioctl (fd, EVIOCGNAME (sizeof (name)), name);
+ //printf ("Reading From : %s (%s)\n", argv[1], name);
+
+
+ return 0;
+}
+
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
deleted file mode 100644
index 14ca523..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared
+++ /dev/null
Binary files differ
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
deleted file mode 100644
index cd79cfa..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk
+++ /dev/null
@@ -1,96 +0,0 @@
-EXEC = inv_mpu_iio$(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 += $(ANDROID_COMPILE)
-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/mpu_iio/build/filelist.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
deleted file mode 100644
index 8a3977a..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk
+++ /dev/null
@@ -1,12 +0,0 @@
-#### filelist.mk for mpu_iio ####
-
-# headers
-#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h
-HEADERS += $(APP_DIR)/iio_utils.h
-
-# sources
-SOURCES := $(APP_DIR)/mpu_iio.c
-
-INV_SOURCES += $(SOURCES)
-
-VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
deleted file mode 100644
index 773ff2c..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h
+++ /dev/null
@@ -1,643 +0,0 @@
-/* IIO - useful set of util functionality
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- */
-
-/* Made up value to limit allocation sizes */
-#include <string.h>
-#include <stdlib.h>
-#include <ctype.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <dirent.h>
-
-#define IIO_MAX_NAME_LENGTH 30
-
-#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
-#define FORMAT_TYPE_FILE "%s_type"
-
-const char *iio_dir = "/sys/bus/iio/devices/";
-
-/**
- * iioutils_break_up_name() - extract generic name from full channel name
- * @full_name: the full channel name
- * @generic_name: the output generic channel name
- **/
-static int iioutils_break_up_name(const char *full_name,
- char **generic_name)
-{
- char *current;
- char *w, *r;
- char *working;
- current = strdup(full_name);
- working = strtok(current, "_\0");
- w = working;
- r = working;
-
- while (*r != '\0') {
- if (!isdigit(*r)) {
- *w = *r;
- w++;
- }
- r++;
- }
- *w = '\0';
- *generic_name = strdup(working);
- free(current);
-
- return 0;
-}
-
-/**
- * struct iio_channel_info - information about a given channel
- * @name: channel name
- * @generic_name: general name for channel type
- * @scale: scale factor to be applied for conversion to si units
- * @offset: offset to be applied for conversion to si units
- * @index: the channel index in the buffer output
- * @bytes: number of bytes occupied in buffer output
- * @mask: a bit mask for the raw output
- * @is_signed: is the raw value stored signed
- * @enabled: is this channel enabled
- **/
-struct iio_channel_info {
- char *name;
- char *generic_name;
- float scale;
- float offset;
- unsigned index;
- unsigned bytes;
- unsigned bits_used;
- unsigned shift;
- uint64_t mask;
- unsigned be;
- unsigned is_signed;
- unsigned enabled;
- unsigned location;
-};
-
-/**
- * iioutils_get_type() - find and process _type attribute data
- * @is_signed: output whether channel is signed
- * @bytes: output how many bytes the channel storage occupies
- * @mask: output a bit mask for the raw data
- * @be: big endian
- * @device_dir: the iio device directory
- * @name: the channel name
- * @generic_name: the channel type name
- **/
-inline int iioutils_get_type(unsigned *is_signed,
- unsigned *bytes,
- unsigned *bits_used,
- unsigned *shift,
- uint64_t *mask,
- unsigned *be,
- const char *device_dir,
- const char *name,
- const char *generic_name)
-{
- FILE *sysfsfp;
- int ret;
- DIR *dp;
- char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
- char signchar, endianchar;
- unsigned padint;
- const struct dirent *ent;
-
- ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_ret;
- }
- ret = asprintf(&builtname, FORMAT_TYPE_FILE, name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_free_scan_el_dir;
- }
- ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_free_builtname;
- }
-
- dp = opendir(scan_el_dir);
- if (dp == NULL) {
- ret = -errno;
- goto error_free_builtname_generic;
- }
- while (ent = readdir(dp), ent != NULL)
- /*
- * Do we allow devices to override a generic name with
- * a specific one?
- */
- if ((strcmp(builtname, ent->d_name) == 0) ||
- (strcmp(builtname_generic, ent->d_name) == 0)) {
- ret = asprintf(&filename,
- "%s/%s", scan_el_dir, ent->d_name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_closedir;
- }
- sysfsfp = fopen(filename, "r");
- if (sysfsfp == NULL) {
- printf("failed to open %s\n", filename);
- ret = -errno;
- goto error_free_filename;
- }
-
- ret = fscanf(sysfsfp,
- "%ce:%c%u/%u>>%u",
- &endianchar,
- &signchar,
- bits_used,
- &padint, shift);
- if (ret < 0) {
- printf("failed to pass scan type description\n");
- return ret;
- }
- *be = (endianchar == 'b');
- *bytes = padint / 8;
- if (*bits_used == 64)
- *mask = ~0;
- else
- *mask = (1 << *bits_used) - 1;
- if (signchar == 's')
- *is_signed = 1;
- else
- *is_signed = 0;
- fclose(sysfsfp);
- free(filename);
-
- filename = 0;
- }
-error_free_filename:
- if (filename)
- free(filename);
-error_closedir:
- closedir(dp);
-error_free_builtname_generic:
- free(builtname_generic);
-error_free_builtname:
- free(builtname);
-error_free_scan_el_dir:
- free(scan_el_dir);
-error_ret:
- return ret;
-}
-
-inline int iioutils_get_param_float(float *output,
- const char *param_name,
- const char *device_dir,
- const char *name,
- const char *generic_name)
-{
- FILE *sysfsfp;
- int ret;
- DIR *dp;
- char *builtname, *builtname_generic;
- char *filename = NULL;
- const struct dirent *ent;
-
- ret = asprintf(&builtname, "%s_%s", name, param_name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_ret;
- }
- ret = asprintf(&builtname_generic,
- "%s_%s", generic_name, param_name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_free_builtname;
- }
- dp = opendir(device_dir);
- if (dp == NULL) {
- ret = -errno;
- goto error_free_builtname_generic;
- }
- while (ent = readdir(dp), ent != NULL)
- if ((strcmp(builtname, ent->d_name) == 0) ||
- (strcmp(builtname_generic, ent->d_name) == 0)) {
- ret = asprintf(&filename,
- "%s/%s", device_dir, ent->d_name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_closedir;
- }
- sysfsfp = fopen(filename, "r");
- if (!sysfsfp) {
- ret = -errno;
- goto error_free_filename;
- }
- fscanf(sysfsfp, "%f", output);
- break;
- }
-error_free_filename:
- if (filename)
- free(filename);
-error_closedir:
- closedir(dp);
-error_free_builtname_generic:
- free(builtname_generic);
-error_free_builtname:
- free(builtname);
-error_ret:
- return ret;
-}
-
-/**
- * bsort_channel_array_by_index() - reorder so that the array is in index order
- *
- **/
-
-inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array,
- int cnt)
-{
-
- struct iio_channel_info temp;
- int x, y;
-
- for (x = 0; x < cnt; x++)
- for (y = 0; y < (cnt - 1); y++)
- if ((*ci_array)[y].index > (*ci_array)[y+1].index) {
- temp = (*ci_array)[y + 1];
- (*ci_array)[y + 1] = (*ci_array)[y];
- (*ci_array)[y] = temp;
- }
-}
-
-/**
- * build_channel_array() - function to figure out what channels are present
- * @device_dir: the IIO device directory in sysfs
- * @
- **/
-inline int build_channel_array(const char *device_dir,
- struct iio_channel_info **ci_array,
- int *counter)
-{
- DIR *dp;
- FILE *sysfsfp;
- int count, i;
- struct iio_channel_info *current;
- int ret;
- const struct dirent *ent;
- char *scan_el_dir;
- char *filename;
-
- *counter = 0;
- ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_ret;
- }
- dp = opendir(scan_el_dir);
- if (dp == NULL) {
- ret = -errno;
- goto error_free_name;
- }
- while (ent = readdir(dp), ent != NULL)
- if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
- "_en") == 0) {
- ret = asprintf(&filename,
- "%s/%s", scan_el_dir, ent->d_name);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_close_dir;
- }
- sysfsfp = fopen(filename, "r");
- if (sysfsfp == NULL) {
- ret = -errno;
- free(filename);
- goto error_close_dir;
- }
- fscanf(sysfsfp, "%u", &ret);
- printf("%s, %d\n", filename, ret);
- if (ret == 1)
- (*counter)++;
- fclose(sysfsfp);
- free(filename);
- }
- *ci_array = malloc(sizeof(**ci_array) * (*counter));
- if (*ci_array == NULL) {
- ret = -ENOMEM;
- goto error_close_dir;
- }
- closedir(dp);
- dp = opendir(scan_el_dir);
- //seekdir(dp, 0);
- count = 0;
- while (ent = readdir(dp), ent != NULL) {
- if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
- "_en") == 0) {
- current = &(*ci_array)[count++];
- ret = asprintf(&filename,
- "%s/%s", scan_el_dir, ent->d_name);
- if (ret < 0) {
- ret = -ENOMEM;
- /* decrement count to avoid freeing name */
- count--;
- goto error_cleanup_array;
- }
- sysfsfp = fopen(filename, "r");
- if (sysfsfp == NULL) {
- free(filename);
- ret = -errno;
- goto error_cleanup_array;
- }
- fscanf(sysfsfp, "%u", ¤t->enabled);
- fclose(sysfsfp);
-
- if (!current->enabled) {
- free(filename);
- count--;
- continue;
- }
-
- current->scale = 1.0;
- current->offset = 0;
- current->name = strndup(ent->d_name,
- strlen(ent->d_name) -
- strlen("_en"));
- if (current->name == NULL) {
- free(filename);
- ret = -ENOMEM;
- goto error_cleanup_array;
- }
- /* Get the generic and specific name elements */
- ret = iioutils_break_up_name(current->name,
- ¤t->generic_name);
- if (ret) {
- free(filename);
- goto error_cleanup_array;
- }
- ret = asprintf(&filename,
- "%s/%s_index",
- scan_el_dir,
- current->name);
- if (ret < 0) {
- free(filename);
- ret = -ENOMEM;
- goto error_cleanup_array;
- }
- sysfsfp = fopen(filename, "r");
- fscanf(sysfsfp, "%u", ¤t->index);
- fclose(sysfsfp);
- free(filename);
- /* Find the scale */
- ret = iioutils_get_param_float(¤t->scale,
- "scale",
- device_dir,
- current->name,
- current->generic_name);
- if (ret < 0)
- goto error_cleanup_array;
- ret = iioutils_get_param_float(¤t->offset,
- "offset",
- device_dir,
- current->name,
- current->generic_name);
- if (ret < 0)
- goto error_cleanup_array;
- ret = iioutils_get_type(¤t->is_signed,
- ¤t->bytes,
- ¤t->bits_used,
- ¤t->shift,
- ¤t->mask,
- ¤t->be,
- device_dir,
- current->name,
- current->generic_name);
- }
- }
-
- closedir(dp);
- /* reorder so that the array is in index order */
- bsort_channel_array_by_index(ci_array, *counter);
-
- return 0;
-
-error_cleanup_array:
- for (i = count - 1; i >= 0; i--)
- free((*ci_array)[i].name);
- free(*ci_array);
-error_close_dir:
- closedir(dp);
-error_free_name:
- free(scan_el_dir);
-error_ret:
- return ret;
-}
-
-inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
-{
- int ret;
- FILE *sysfsfp;
- int test;
- char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
- if (temp == NULL)
- return -ENOMEM;
- sprintf(temp, "%s/%s", basedir, filename);
- sysfsfp = fopen(temp, "w");
- if (sysfsfp == NULL) {
- printf("failed to open %s\n", temp);
- ret = -errno;
- goto error_free;
- }
- fprintf(sysfsfp, "%d", val);
- fclose(sysfsfp);
- if (verify) {
- sysfsfp = fopen(temp, "r");
- if (sysfsfp == NULL) {
- printf("failed to open %s\n", temp);
- ret = -errno;
- goto error_free;
- }
- fscanf(sysfsfp, "%d", &test);
- if (test != val) {
- printf("Possible failure in int write %d to %s%s\n",
- val,
- basedir,
- filename);
- ret = -1;
- }
- }
-error_free:
- free(temp);
- return ret;
-}
-
-int write_sysfs_int(char *filename, char *basedir, int val)
-{
- return _write_sysfs_int(filename, basedir, val, 0);
-}
-
-int write_sysfs_int_and_verify(char *filename, char *basedir, int val)
-{
- return _write_sysfs_int(filename, basedir, val, 1);
-}
-
-int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
-{
- int ret = 0;
- FILE *sysfsfp;
- char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
- if (temp == NULL) {
- printf("Memory allocation failed\n");
- return -ENOMEM;
- }
- sprintf(temp, "%s/%s", basedir, filename);
- sysfsfp = fopen(temp, "w");
- if (sysfsfp == NULL) {
- printf("Could not open %s\n", temp);
- ret = -errno;
- goto error_free;
- }
- fprintf(sysfsfp, "%s", val);
- fclose(sysfsfp);
- if (verify) {
- sysfsfp = fopen(temp, "r");
- if (sysfsfp == NULL) {
- printf("could not open file to verify\n");
- ret = -errno;
- goto error_free;
- }
- fscanf(sysfsfp, "%s", temp);
- if (strcmp(temp, val) != 0) {
- printf("Possible failure in string write of %s "
- "Should be %s "
- "written to %s\%s\n",
- temp,
- val,
- basedir,
- filename);
- ret = -1;
- }
- }
-error_free:
- free(temp);
-
- return ret;
-}
-
-/**
- * write_sysfs_string_and_verify() - string write, readback and verify
- * @filename: name of file to write to
- * @basedir: the sysfs directory in which the file is to be found
- * @val: the string to write
- **/
-int write_sysfs_string_and_verify(char *filename, char *basedir, char *val)
-{
- return _write_sysfs_string(filename, basedir, val, 1);
-}
-
-int write_sysfs_string(char *filename, char *basedir, char *val)
-{
- return _write_sysfs_string(filename, basedir, val, 0);
-}
-
-int read_sysfs_posint(char *filename, char *basedir)
-{
- int ret;
- FILE *sysfsfp;
- char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
- if (temp == NULL) {
- printf("Memory allocation failed");
- return -ENOMEM;
- }
- sprintf(temp, "%s/%s", basedir, filename);
- sysfsfp = fopen(temp, "r");
- if (sysfsfp == NULL) {
- ret = -errno;
- goto error_free;
- }
- fscanf(sysfsfp, "%d\n", &ret);
- fclose(sysfsfp);
-error_free:
- free(temp);
- return ret;
-}
-
-int read_sysfs_float(char *filename, char *basedir, float *val)
-{
- float ret = 0;
- FILE *sysfsfp;
- char *temp = malloc(strlen(basedir) + strlen(filename) + 2);
- if (temp == NULL) {
- printf("Memory allocation failed");
- return -ENOMEM;
- }
- sprintf(temp, "%s/%s", basedir, filename);
- sysfsfp = fopen(temp, "r");
- if (sysfsfp == NULL) {
- ret = -errno;
- goto error_free;
- }
- fscanf(sysfsfp, "%f\n", val);
- fclose(sysfsfp);
-error_free:
- free(temp);
- return ret;
-}
-int enable(const char *device_dir,
- struct iio_channel_info **ci_array,
- int *counter)
-{
- DIR *dp;
- int ret;
- const struct dirent *ent;
- char *scan_el_dir;
-
- *counter = 0;
- ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_ret;
- }
- dp = opendir(scan_el_dir);
- if (dp == NULL) {
- ret = -errno;
- goto error_free_name;
- }
- while (ent = readdir(dp), ent != NULL)
- if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"),
- "_en") == 0) {
- write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 1);
- }
- return 0;
-error_ret:
-error_free_name:
- return -1;
-}
-int disable_q_out(const char *device_dir,
- struct iio_channel_info **ci_array,
- int *counter) {
- DIR *dp;
- int ret;
- const struct dirent *ent;
- char *scan_el_dir;
-
- *counter = 0;
- ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir);
- if (ret < 0) {
- ret = -ENOMEM;
- goto error_ret;
- }
- dp = opendir(scan_el_dir);
- if (dp == NULL) {
- ret = -errno;
- goto error_free_name;
- }
- while (ent = readdir(dp), ent != NULL)
- if (strncmp(ent->d_name, "in_quaternion", strlen("in_quaternion")) == 0) {
- write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 0);
- }
- return 0;
-error_ret:
-error_free_name:
- return -1;
-
-}
-
diff --git a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
deleted file mode 100644
index b3d323c..0000000
--- a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c
+++ /dev/null
@@ -1,685 +0,0 @@
-/* Industrialio buffer test code.
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is primarily intended as an example application.
- * Reads the current buffer setup from sysfs and starts a short capture
- * from the specified device, pretty printing the result after appropriate
- * conversion.
- *
- * Command line parameters
- * generic_buffer -n <device_name> -t <trigger_name>
- * If trigger name is not specified the program assumes you want a dataready
- * trigger associated with the device and goes looking for it.
- *
- */
-
-#include <unistd.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <sys/stat.h>
-#include <dirent.h>
-#include <linux/types.h>
-#include <string.h>
-#include <poll.h>
-#include "iio_utils.h"
-#include "ml_load_dmp.h"
-#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
- * @num_channels: size of the channel info array
- *
- * Has the side effect of filling the channels[i].location values used
- * in processing the buffer output.
- **/
-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;
-}
-
-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);
- }
-}
-/**
- * process_scan() - print out the values in SI units
- * @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
- **/
-void process_scan(char *data,
- 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");
-}
-
-#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");
- if (orient & 0x02)
- printf("INV_X_DOWN\n");
- if (orient & 0x04)
- printf("INV_Y_UP\n");
- if (orient & 0x08)
- printf("INV_Y_DOWN\n");
- if (orient & 0x10)
- printf("INV_Z_UP\n");
- if (orient & 0x20)
- printf("INV_Z_DOWN\n");
- if (orient & 0x40)
- printf("INV_ORIENTATION_FLIP\n");
-}
-
-void HandleTap(int tap)
-{
- int tap_dir = tap/8;
- int tap_num = tap%8 + 1;
-
- switch (tap_dir) {
- case 1:
- printf("INV_TAP_AXIS_X_POS\n");
- break;
- case 2:
- printf("INV_TAP_AXIS_X_NEG\n");
- break;
- case 3:
- printf("INV_TAP_AXIS_Y_POS\n");
- break;
- case 4:
- printf("INV_TAP_AXIS_Y_NEG\n");
- break;
- case 5:
- printf("INV_TAP_AXIS_Z_POS\n");
- break;
- case 6:
- printf("INV_TAP_AXIS_Z_NEG\n");
- break;
- default:
- break;
- }
- printf("Tap number: %d\n", tap_num);
-}
-#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, 2);
- if (ret < 0)
- return;
- ret = write_sysfs_int("sampling_frequency", sysfs_path, 200);
- if (ret < 0)
- return;
- ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0);
- if (ret < 0)
- return;
- sprintf(dmp_path, "%s/dmp_firmware", dev_path);
- if ((fd = fopen(dmp_path, "wb")) < 0 ) {
- perror("dmp fail");
- }
- inv_load_dmp(fd);
- fclose(fd);
- printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path));
- 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;
- /* 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;
- 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;
-#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;
- /* -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);
-/*
- 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);
-error_free_data:
- free(data);
-exit_here:
- /* Stop the ring buffer */
- ret = write_sysfs_int("enable", buf_dir_name, 0);
-
-error_free_buf_dir_name:
- free(buf_dir_name);
-error_free_triggername:
- if (datardytrigger)
- free(trigger_name);
-error_ret:
- return ret;
-}
diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
index 01d1425..33c9eef 100644
--- a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
+++ b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared
Binary files differ
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 ed5fbf6..3a055cc 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,7 +18,6 @@
include $(INV_ROOT)/software/build/android/common.mk
CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += $(ANDROID_COMPILE)
CFLAGS += -Wall
CFLAGS += -fpic
CFLAGS += -nostdlib
@@ -50,8 +49,20 @@
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 += $(ANDROID_LINK_EXECUTABLE)
+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
LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
@@ -76,7 +87,7 @@
$(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)
+ $(LINK) $(PRE_LFLAGS) $(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 2fe2cff..4f9996c 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
@@ -22,19 +22,13 @@
#include "ml_math_func.h"
#include "storage_manager.h"
#include "ml_stored_data.h"
-#include "ml_sysfs_helper.h"
#ifndef ABS
#define ABS(x)(((x) >= 0) ? (x) : -(x))
#endif
-//#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*))
-
/** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53395
+#define INV_DB_SAVE_KEY 53394
#define FALSE 0
#define TRUE 1
@@ -44,29 +38,26 @@
#define COMPASS_PASS_STATUS_BIT 0x04
typedef union {
- long l;
+ long l;
int i;
} bias_dtype;
-char *sysfs_names_ptr;
-
-struct sysfs_attrbs {
+struct inv_sysfs_names_s {
char *enable;
char *power_state;
- char *dmp_on;
- char *dmp_int_on;
char *self_test;
char *temperature;
- char *gyro_enable;
- char *gyro_x_bias;
- char *gyro_y_bias;
- char *gyro_z_bias;
- char *accel_enable;
- char *accel_x_bias;
- char *accel_y_bias;
- char *accel_z_bias;
- char *compass_enable;
-} mpu;
+ char *accl_bias;
+};
+
+const struct inv_sysfs_names_s mpu= {
+ /* MPU6050 & MPU9150 */
+ .enable = "/sys/class/invensense/mpu/enable",
+ .power_state = "/sys/class/invensense/mpu/power_state",
+ .self_test = "/sys/class/invensense/mpu/self_test",
+ .temperature = "/sys/class/invensense/mpu/temperature",
+ .accl_bias = "/sys/class/invensense/mpu/accl_bias"
+};
struct inv_db_save_t {
/** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
@@ -80,123 +71,46 @@
/** Temperature when accel bias was stored. */
long accel_temp;
long gyro_temp_slope[3];
- /** Sensor Accuracy */
- int gyro_accuracy;
- int accel_accuracy;
- int compass_accuracy;
};
static struct inv_db_save_t save_data;
-/** This function receives the data that was stored in non-volatile memory
- between power off */
+/** This function receives the data that was stored in non-volatile memory between power off */
static inv_error_t inv_db_load_func(const unsigned char *data)
{
memcpy(&save_data, data, sizeof(save_data));
return INV_SUCCESS;
}
-/** This function returns the data to be stored in non-volatile memory between
- power off */
+/** This function returns the data to be stored in non-volatile memory between power off */
static inv_error_t inv_db_save_func(unsigned char *data)
{
memcpy(data, &save_data, sizeof(save_data));
return INV_SUCCESS;
}
-/** read a sysfs entry that represents an integer */
-int read_sysfs_int(char *filename, int *var)
+int inv_sysfs_write(char *filename, long data)
{
- int res=0;
FILE *fp;
+ int count;
- fp = fopen(filename, "r");
- if (fp != NULL) {
- fscanf(fp, "%d\n", var);
- fclose(fp);
- } else {
- MPL_LOGE("inv_self_test: ERR open file to read");
- res= -1;
- }
- return res;
-}
-
-/** write a sysfs entry that represents an integer */
-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("inv_self_test: ERR open file to write");
- res= -1;
- }
- return res;
-}
-
-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("inv_self_test: couldn't alloc mem for sysfs paths");
+ if (!filename)
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.self_test, "%s%s", sysfs_path, "/self_test");
- sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
-
- sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
- sprintf(mpu.gyro_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias");
- sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias");
- sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias");
-
- sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
- sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias");
- sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias");
- sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias");
-
- sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable");
-
-#if 0
- // test print sysfs paths
- dptr = (char**)&mpu;
- for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
- MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++);
- }
-#endif
- return 0;
+ fp = fopen(filename, "w");
+ if (!fp)
+ return -errno;
+ count = fprintf(fp, "%ld", data);
+ fclose(fp);
+ return count;
}
-/*******************************************************************************
- * M a i n S e l f T e s t
- ******************************************************************************/
+/**
+ * Main Self test
+ */
int main(int argc, char **argv)
{
FILE *fptr;
- int self_test_status = 0;
+ int self_test_status;
inv_error_t result;
bias_dtype gyro_bias[3];
bias_dtype accel_bias[3];
@@ -205,133 +119,76 @@
int axis_sign = 1;
unsigned char *buffer;
long timestamp;
- int temperature = 0;
- bool compass_present = TRUE;
+ int temperature=0;
- result = inv_init_sysfs_attributes();
- if (result)
- return -1;
-
+ // Initialize storage manager
inv_init_storage_manager();
// Clear out data.
memset(&save_data, 0, sizeof(save_data));
- memset(gyro_bias, 0, sizeof(gyro_bias));
- memset(accel_bias, 0, sizeof(accel_bias));
+ memset(gyro_bias,0, sizeof(gyro_bias));
+ memset(accel_bias,0, sizeof(accel_bias));
// Register packet to be saved.
- result = inv_register_load_store(
- inv_db_load_func, inv_db_save_func,
- sizeof(save_data), INV_DB_SAVE_KEY);
+ result = inv_register_load_store(inv_db_load_func, inv_db_save_func,
+ sizeof(save_data),
+ INV_DB_SAVE_KEY);
// Power ON MPUxxxx chip
- if (write_sysfs_int(mpu.power_state, 1) < 0) {
- printf("Self-Test:ERR-Failed to set power state=1\n");
+ if (inv_sysfs_write(mpu.power_state, 1) <0) {
+ printf("ERR- Failed to set power state=1\n");
} else {
// Note: Driver turns on power automatically when self-test invoked
}
- // Disable Master enable
- if (write_sysfs_int(mpu.enable, 0) < 0) {
- printf("Self-Test:ERR-Failed to disable master enable\n");
- }
-
- // Disable DMP
- if (write_sysfs_int(mpu.dmp_on, 0) < 0) {
- printf("Self-Test:ERR-Failed to disable DMP\n");
- }
-
- // Enable Accel
- if (write_sysfs_int(mpu.accel_enable, 1) < 0) {
- printf("Self-Test:ERR-Failed to enable accel\n");
- }
-
- // Enable Gyro
- if (write_sysfs_int(mpu.gyro_enable, 1) < 0) {
- printf("Self-Test:ERR-Failed to enable gyro\n");
- }
-
- // Enable Compass
- if (write_sysfs_int(mpu.compass_enable, 1) < 0) {
-#ifdef DEBUG_PRINT
- printf("Self-Test:ERR-Failed to enable compass\n");
-#endif
- compass_present= FALSE;
- }
-
fptr = fopen(mpu.self_test, "r");
- if (!fptr) {
- printf("Self-Test:ERR-Couldn't invoke self-test\n");
- result = -1;
- goto free_sysfs_storage;
- }
-
- // Invoke self-test
- fscanf(fptr, "%d", &self_test_status);
- if (compass_present == TRUE) {
- printf("Self-Test:Self test result- "
- "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
- (self_test_status & GYRO_PASS_STATUS_BIT),
- (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1,
- (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2);
- } else {
- printf("Self-Test:Self test result- "
- "Gyro passed= %x, Accel passed= %x\n",
- (self_test_status & GYRO_PASS_STATUS_BIT),
- (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1);
- }
- fclose(fptr);
-
- if (self_test_status & GYRO_PASS_STATUS_BIT) {
- // Read Gyro Bias
- if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 ||
- read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 ||
- read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) {
- memset(gyro_bias, 0, sizeof(gyro_bias));
- printf("Self-Test:Failed to read Gyro bias\n");
- } else {
- save_data.gyro_accuracy = 3;
-#ifdef DEBUG_PRINT
- printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n",
- gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
-#endif
- }
- } else {
- printf("Self-Test:Failed Gyro self-test\n");
- }
-
- if (self_test_status & ACCEL_PASS_STATUS_BIT) {
- // Read Accel Bias
- if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 ||
- read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 ||
- read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) {
- memset(accel_bias,0, sizeof(accel_bias));
- printf("Self-Test:Failed to read Accel bias\n");
- } else {
- save_data.accel_accuracy = 3;
-#ifdef DEBUG_PRINT
- printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n",
- accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
-#endif
- }
- } else {
- printf("Self-Test:Failed Accel self-test\n");
- }
-
- if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) {
- printf("Self-Test:Failed Gyro and Accel self-test, "
- "nothing left to do\n");
- result = -1;
- goto free_sysfs_storage;
- }
-
- // Read temperature
- fptr= fopen(mpu.temperature, "r");
if (fptr != NULL) {
- fscanf(fptr,"%d %ld", &temperature, ×tamp);
+ // Invoke self-test and read gyro bias
+ fscanf(fptr, "%d,%d,%d,%d",
+ &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status);
+
+ printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n",
+ (self_test_status & GYRO_PASS_STATUS_BIT),
+ (self_test_status & ACCEL_PASS_STATUS_BIT) >>1,
+ (self_test_status & COMPASS_PASS_STATUS_BIT) >>2);
+ printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i);
fclose(fptr);
+
+ if (!(self_test_status & GYRO_PASS_STATUS_BIT)) {
+ // Reset gyro bias data if gyro self-test failed
+ memset(gyro_bias,0, sizeof(gyro_bias));
+ printf("Self-Test:Failed Gyro self-test\n");
+ }
+
+ if (self_test_status & ACCEL_PASS_STATUS_BIT) {
+ // Read Accel Bias
+ fptr= fopen(mpu.accl_bias, "r");
+ if (fptr != NULL) {
+ fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i);
+ printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i);
+ fclose(fptr);
+ } else {
+ printf("Self-Test:ERR-Couldn't read accel bias\n");
+ }
+ } else {
+ memset(accel_bias,0, sizeof(accel_bias));
+ printf("Self-Test:Failed Accel self-test\n");
+ }
+
+ // Read temperature
+ if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT))
+ {
+ fptr= fopen(mpu.temperature, "r");
+ if (fptr != NULL) {
+ fscanf(fptr,"%d %ld", &temperature, ×tamp);
+ fclose(fptr);
+ } else {
+ printf("Self-Test:ERR-Couldn't read temperature\n");
+ }
+ }
+
} else {
- printf("Self-Test:ERR-Couldn't read temperature\n");
+ printf("Self-Test:ERR-Couldn't invoke self-test\n");
}
// When we read gyro bias, the bias is in raw units scaled by 1000.
@@ -368,16 +225,12 @@
save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f);
#if 1
- printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n",
- save_data.accel_bias[0], save_data.accel_bias[1],
- save_data.accel_bias[2]);
- printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n",
- save_data.gyro_bias[0], save_data.gyro_bias[1],
- save_data.gyro_bias[2]);
- printf("Self-Test:Gyro temperature @ time stored %ld\n",
- save_data.gyro_temp);
- printf("Self-Test:Accel temperature @ time stored %ld\n",
- save_data.accel_temp);
+ printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0],
+ save_data.accel_bias[1], save_data.accel_bias[2]);
+ printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0],
+ save_data.gyro_bias[1], save_data.gyro_bias[2]);
+ printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp);
+ printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp);
#endif
// Get size of packet to store.
@@ -387,30 +240,25 @@
buffer = (unsigned char *)malloc(packet_sz + 10);
if (buffer == NULL) {
printf("Self-Test:Can't allocate buffer\n");
- result = -1;
- goto free_sysfs_storage;
+ return -1;
}
- // Store the data
result = inv_save_mpl_states(buffer, packet_sz);
if (result) {
- result = -1;
+ result= -1;
} else {
fptr= fopen(MLCAL_FILE, "wb+");
if (fptr != NULL) {
fwrite(buffer, 1, packet_sz, fptr);
fclose(fptr);
} else {
- printf("Self-Test:ERR- Can't open calibration file to write - %s\n",
+ printf("Self-Test:ERR- Can't open calibration file to write - %s\n",
MLCAL_FILE);
- result = -1;
+ result= -1;
}
}
free(buffer);
-
-free_sysfs_storage:
- free(sysfs_names_ptr);
return result;
}
