diff options
| author | Ramanan Rajeswaran <ramanan@google.com> | 2012-09-26 14:24:48 -0700 |
|---|---|---|
| committer | Android (Google) Code Review <android-gerrit@google.com> | 2012-09-26 14:24:48 -0700 |
| commit | 64ca18f95225d0a86f7ccfd1d21c23971b9f77ae (patch) | |
| tree | d00916dda47dd39f3d625c834e2f2e04782559f1 | |
| parent | 6a4d9a48ffde124c498496f6dae1e77c2cae6864 (diff) | |
| download | android_hardware_invensense-64ca18f95225d0a86f7ccfd1d21c23971b9f77ae.tar.gz android_hardware_invensense-64ca18f95225d0a86f7ccfd1d21c23971b9f77ae.tar.bz2 android_hardware_invensense-64ca18f95225d0a86f7ccfd1d21c23971b9f77ae.zip | |
Revert "Official MA 5.1.2 - RC11 Release"
This reverts commit 6a4d9a48ffde124c498496f6dae1e77c2cae6864
Change-Id: I85c4b326a2adf930fcd4a64e4c4f36e99f4fa4cb
105 files changed, 8921 insertions, 6141 deletions
@@ -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 @@ else # release builds, default 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 += libcutils 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 @@ LOCAL_PRELINK_MODULE := false 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 @@ include $(CLEAR_VARS) LOCAL_MODULE := libmllite LOCAL_SRC_FILES := libmllite.so LOCAL_MODULE_TAGS := optional -LOCAL_MODULE_OWNER := invensense LOCAL_MODULE_SUFFIX := .so LOCAL_MODULE_CLASS := SHARED_LIBRARIES LOCAL_MODULE_PATH := $(TARGET_OUT)/lib 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 @@ CompassSensor::CompassSensor() 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 @@ CompassSensor::CompassSensor() LOGE("HAL:Couldn't read compass mounting matrix"); } - if (!isIntegrated()) { - enable(ID_M, 0); - } + enable(ID_M, 0); } CompassSensor::~CompassSensor() @@ -131,11 +130,11 @@ int CompassSensor::getFd() const * @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 CompassSensor::enable(int32_t handle, int en) 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 @@ int CompassSensor::enable(int32_t handle, int en) 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 @@ int CompassSensor::enable(int32_t handle, int en) 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 @@ int CompassSensor::enable(int32_t handle, int en) 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 @@ int CompassSensor::enable(int32_t handle, int en) 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 @@ void CompassSensor::processCompassEvent(const input_event *event) mCachedCompassData[2] = event->value; break; } - - mCompassTimestamp = + + mCompassTimestamp = (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L; } @@ -263,7 +262,7 @@ long CompassSensor::getSensitivity() 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 @@ int CompassSensor::readSample(long *data, int64_t *timestamp) /** * @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 @@ void CompassSensor::fillList(struct sensor_t *list) 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 @@ int CompassSensor::inv_init_sysfs_attributes(void) // 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 @@ int CompassSensor::inv_init_sysfs_attributes(void) #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 @@ static int hertz_request = 200; #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 @@ static struct sensor_t sSensorList[] = {"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 @@ static FILE *logfile = NULL; * 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * mAccelScale(2), mPendingMask(0), mSensorMask(0), + mGyroOrientation{0}, + mAccelOrientation{0}, mFeatureActiveMask(0) { VFUNC_LOG; @@ -185,7 +171,7 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * #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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * /* 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * /* 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * 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 @@ int MPLSensor::inv_constructor_default_enable() 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 @@ int MPLSensor::inv_constructor_default_enable() 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 @@ int MPLSensor::inv_constructor_default_enable() 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 @@ int MPLSensor::inv_constructor_default_enable() return result; } + // TODO: double-check for GED tablet result = inv_enable_quat_accuracy_monitor(); if (result) { LOG_RESULT_LOCATION(result); @@ -574,15 +513,15 @@ void MPLSensor::inv_set_device_properties() /* 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 @@ void MPLSensor::loadDMP() 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 @@ void MPLSensor::inv_get_sensors_orientation() 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 @@ void MPLSensor::inv_get_sensors_orientation() } // 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 @@ MPLSensor::~MPLSensor() /* 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 @@ MPLSensor::~MPLSensor() 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 @@ MPLSensor::~MPLSensor() #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) +#else +// TODO: ID_M = 2 even for 3rd-party solution #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) +#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 MPLSensor::setGyroInitialState() 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 @@ int MPLSensor::setGyroInitialState() return 0; } -/* this applies to BMA250 Input Subsystem Driver only */ +/* this applies to BMA250 only */ int MPLSensor::setAccelInitialState() { VFUNC_LOG; @@ -796,7 +737,7 @@ int MPLSensor::onPower(int en) 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 @@ int MPLSensor::onPower(int en) } 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 @@ int MPLSensor::onDMP(int en) //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 @@ int MPLSensor::enableLPQuaternion(int en) 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 MPLSensor::enableQuaternionData(int en) 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 MPLSensor::masterEnable(int en) 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 MPLSensor::enableGyro(int en) 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 @@ int MPLSensor::enableGyro(int en) LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); inv_gyro_was_turned_off(); } else { - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 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 @@ int MPLSensor::enableGyro(int en) 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 @@ int MPLSensor::enableGyro(int en) 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 MPLSensor::enableAccel(int en) int res; - /* need to also turn on/off the master enable */ - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - en, mpu.accel_enable, getTimestamp()); - 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 @@ int MPLSensor::enableAccel(int en) 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 @@ int MPLSensor::enableAccel(int en) 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 @@ int MPLSensor::enableAccel(int en) } if (!en && USE_THIRD_PARTY_ACCEL == 0) { - } + } if(USE_THIRD_PARTY_ACCEL == 1 && en) { - //setAccelInitialState();// BMA250 + setAccelInitialState(); // BMA250 } return res; } @@ -1139,7 +1040,7 @@ int MPLSensor::enableCompass(int en) 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 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { 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 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { 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 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { 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; - } - } - else { - res = onPower(0); - if(res < 0) { - return res; - } - } - - if (!cal_stored) { - storeCalibration(); - cal_stored = 1; + res = onPower(0); + if(res < 0) { + return res; } - } - } 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 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { /* 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 @@ int MPLSensor::gyroHandler(sensors_event_t* s) 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 @@ int MPLSensor::accelHandler(sensors_event_t* s) 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 @@ int MPLSensor::compassHandler(sensors_event_t* s) s->magnetic.v, &s->magnetic.status, &s->timestamp); LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d", s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update); - mCompassAccuracy = s->magnetic.status; return update; } @@ -1509,17 +1317,9 @@ int MPLSensor::enable(int32_t handle, int en) 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 @@ int MPLSensor::enable(int32_t handle, int en) what = Gyro; sname = "Gyro"; break; - case ID_RG: - what = RawGyro; - sname = "RawGyro"; - break; case ID_GR: what = Gravity; sname = "Gravity"; @@ -1558,10 +1354,11 @@ int MPLSensor::enable(int32_t handle, int en) 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 @@ int MPLSensor::enable(int32_t handle, int en) 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 @@ int MPLSensor::enable(int32_t handle, int en) switch (what) { case Gyro: - case RawGyro: case Accelerometer: case MagneticField: if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) | @@ -1625,7 +1422,7 @@ int MPLSensor::enable(int32_t handle, int en) // 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 MPLSensor::setDelay(int32_t handle, int64_t ns) 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 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) } #endif - if (uint32_t(what) >= NUMSENSORS) + if (uint32_t(what) >= numSensors) return -EINVAL; if (ns < 0) @@ -1708,6 +1500,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) 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 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) switch (what) { case Gyro: - case RawGyro: case Accelerometer: for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); i++) { @@ -1737,7 +1529,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) 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 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) 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 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) break; } + // TODO: disabled for GED tablet // pthread_mutex_lock(&mHALMutex); int res = update_delay(); // pthread_mutex_unlock(&mHALMutex); @@ -1775,31 +1562,33 @@ int MPLSensor::update_delay() { 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 @@ int MPLSensor::update_delay() { 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 MPLSensor::update_delay() { 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 @@ int MPLSensor::update_delay() { //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 @@ int MPLSensor::update_delay() { } 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 @@ int MPLSensor::update_delay() { 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 @@ int MPLSensor::update_delay() { } 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 @@ int MPLSensor::update_delay() { } + /* + 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 @@ int MPLSensor::readAccelEvents(sensors_event_t* data, int count) 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 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count) } // 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 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count) 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 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { 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 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { ((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 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { } } - 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 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { 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]); - } + // 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); + 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 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { 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 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count) { 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 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count) 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 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count) 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 @@ int MPLSensor::getCompassFd() const 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 @@ int MPLSensor::inv_read_temperature(long long *data) 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 @@ int MPLSensor::inv_read_sensor_bias(int fd, long *data) 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 @@ int MPLSensor::populateSensorList(struct sensor_t *list, int len) 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 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) 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 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) 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 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 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 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 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 @@ void MPLSensor::fillRV(struct sensor_t *list) 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 @@ void MPLSensor::fillOrientation(struct sensor_t *list) 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 @@ void MPLSensor::fillGravity( struct sensor_t *list) 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 @@ void MPLSensor::fillLinearAccel(struct sensor_t *list) 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 @@ int MPLSensor::inv_init_sysfs_attributes(void) 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 @@ int MPLSensor::inv_init_sysfs_attributes(void) 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 @@ int MPLSensor::inv_init_sysfs_attributes(void) 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 @@ int MPLSensor::inv_init_sysfs_attributes(void) sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en"); sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en"); +#ifdef THIRD_PARTY_ACCEL //BMA250 + /* same as 'mpu.accel_enable' */ + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/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 enable_sysfs_sensor(int fd, int en) 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 @@ int write_attribute_sensor(int fd, long data) 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 @@ int read_sysfs_int(char *filename, int *var) 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 @@ int write_sysfs_int(char *filename, int var) 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 Binary files differindex c23e5b9..ed13b13 100644 --- a/libsensors_iio/libmllite.so +++ b/libsensors_iio/libmllite.so diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so Binary files differindex 23b4721..e2ab461 100644 --- a/libsensors_iio/libmplmpu.so +++ b/libsensors_iio/libmplmpu.so diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h 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 @@ SHELL = /bin/bash #################################################################################################### ## defines -# Build for Jellybean -BUILD_ANDROID_JELLYBEAN = 1 - ## libraries ## LIB_PREFIX = lib @@ -19,39 +16,23 @@ TARGET ?= android 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 - -ifeq ($(BUILD_ANDROID_JELLYBEAN),1) -ANDROID_COMPILE = -DANDROID_JELLYBEAN=1 -endif +## includes and linker -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 @@ LIB_FOLDERS = $(INV_ROOT)/core/mllite/build/$(TARGET) 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 @@ enum ext_slave_id { }; #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 @@ enum ext_slave_id { * @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 @@ extern "C" { #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 Binary files differdeleted file mode 100644 index acd25a7..0000000 --- a/libsensors_iio/software/core/mllite/build/android/libmllite.so +++ /dev/null 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 @@ MLLITE_DIR = $(INV_ROOT)/software/core/mllite include $(INV_ROOT)/software/build/android/common.mk CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) CFLAGS += -Wall CFLAGS += -fpic CFLAGS += -nostdlib @@ -45,7 +44,12 @@ LLINK += -ldl 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 @@ struct inv_db_save_t { /** 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_data_builder_t inv_data_builder; 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 @@ void inv_turn_off_data_logging() 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 @@ void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, 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 @@ void inv_set_compass_sample_rate(long sample_rate_us) 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 @@ void inv_matrix_vector_mult(const long *A, const long *x, long *y) } /** 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 @@ void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) 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; - - inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); + raw32[0] = (long)sensor->raw[0] << 16; + raw32[1] = (long)sensor->raw[1] << 16; + raw32[2] = (long)sensor->raw[2] << 16; - raw32[0] -= bias[0] >> 1; - raw32[1] -= bias[1] >> 1; - raw32[2] -= bias[2] >> 1; + raw32[0] -= bias[0]; + raw32[1] -= bias[1]; + raw32[2] -= bias[2]; - 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 @@ void inv_set_compass_bias(const long *bias, int accuracy) 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 @@ void inv_set_accel_bias(const long *bias, int accuracy) } } sensors.accel.accuracy = accuracy; - inv_data_builder.save.accel_accuracy = accuracy; - inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); -} - -/** Sets the accel 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 @@ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) 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 @@ void inv_set_gyro_bias(const long *bias, int accuracy) } } 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 @@ inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) 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 @@ inv_error_t inv_build_compass(const long *compass, int status, 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 @@ void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) } } -/** 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 @@ extern "C" { #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 @@ struct inv_single_sensor_t { 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_gyro_bandwidth(int bandwidth_hz); 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_bias(const long *bias, int accuracy); void inv_set_compass_disturbance(int dist); void inv_set_gyro_bias(const long *bias, int accuracy); void inv_set_accel_bias(const long *bias, int accuracy); -void inv_set_accel_accuracy(int accuracy); void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); void inv_get_gyro_bias(long *bias, long *temp); @@ -205,7 +197,6 @@ long inv_get_compass_sensitivity(void); 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 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, 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 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, return status; } -/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. - * So this is: pi / 2^16 / 180 */ -#define GYRO_CONVERSION 2.66316109007924e-007f - -/** Gyroscope calibrated data (rad/s) in body frame. +/** 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_gravity(float *values, int8_t *accuracy, int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, inv_time_t * timestamp) { + /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. + * So this is: pi / 2^16 / 180 */ +#define GYRO_CONVERSION 2.66316109007924e-007f long gyro[3]; int status; @@ -152,30 +143,6 @@ int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, 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_gyroscope_raw(float *values, int8_t *accuracy, 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 inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, 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 @@ static void google_orientation(float *g) 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 @@ inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) use_sensor = 3; } - // Only output 9-axis if all 9 sensors are on. - if (sensor_cal->quat.status & INV_SENSOR_ON) { - // If quaternion sensor is on, gyros are not required as quaternion already has that part - if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { - use_sensor = -1; - } - } else { - if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { - use_sensor = -1; - } - } - switch (use_sensor) { + default: case 0: hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->gyro.timestamp; + 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_error_t inv_start_hal_outputs(void) 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 @@ float compass_low_pass_filter_coeff[5] = */ 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_init_hal_outputs(void) 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 inv_disable_hal_outputs(void) { 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 @@ extern "C" { 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 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, /* bank # 2 */ 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 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 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, - 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 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 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 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_store_cal(unsigned char *calData, size_t length) */ 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; + + inv_get_mpl_state_size(&length); + if (length <= 0) { + MPL_LOGE("Could not get file calibration length - " + "error %d - aborting\n", result); + return result; + } - result = inv_read_cal(&calData, &bytesRead); + 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 @@ inv_error_t inv_load_calibration(void) free_mem_n_exit: inv_free(calData); - return result; + return INV_SUCCESS; } /** @@ -345,7 +345,7 @@ inv_error_t inv_store_calibration(void) 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 @@ inv_error_t inv_store_calibration(void); /* 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 @@ enum PROC_SYSFS_CMD { CMD_GET_DEVICE_NODE }; static char sysfs_path[100]; -static char *chip_name[] = { - "ITG3500", - "MPU6050", - "MPU9150", - "MPU3050", - "MPU6500" -}; +static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"}; static int chip_ind; static int initialized =0; static int status = 0; @@ -32,8 +27,6 @@ static int iio_dev_num = 0; #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 @@ int find_type_by_name(const char *name, const char *type) */ static int parsing_proc_input(int mode, char *name){ const char input[] = "/proc/bus/input/devices"; - char line[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_input_number(const char *name, int *num) */ 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_trigger_path(const char *name) */ 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 @@ extern "C" { #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 @@ void *inv_malloc(unsigned int num_bytes) * @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_create_mutex(HANDLE *mutex) inv_error_t inv_lock_mutex(HANDLE mutex) { int res; - pthread_mutex_t *pm = (pthread_mutex_t *)mutex; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; res = pthread_mutex_lock(pm); - if(res == -1) + if(res == -1) return INV_ERROR_OS_LOCK_FAILED; return INV_SUCCESS; @@ -109,11 +110,11 @@ inv_error_t inv_lock_mutex(HANDLE mutex) * @brief Mutex unlock function * @param mutex mutex handle * @return error code. - */ +**/ inv_error_t inv_unlock_mutex(HANDLE mutex) { int res; - pthread_mutex_t *pm = (pthread_mutex_t *)mutex; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; res = pthread_mutex_unlock(pm); if(res == -1) @@ -130,7 +131,7 @@ inv_error_t inv_unlock_mutex(HANDLE mutex) */ FILE *inv_fopen(char *filename) { - FILE *fp = fopen(filename, "r"); + FILE *fp = fopen(filename,"r"); return fp; } @@ -153,21 +154,22 @@ void inv_fclose(FILE *fp) inv_error_t inv_destroy_mutex(HANDLE handle) { int error; - pthread_mutex_t *pm = (pthread_mutex_t *)handle; + pthread_mutex_t *pm = (pthread_mutex_t*)handle; error = pthread_mutex_destroy(pm); - if (error) + if (error) { return errno; + } free((void*) handle); - + return INV_SUCCESS;} /** * @brief Sleep function. */ -void inv_sleep(int m_secs) +void inv_sleep(int mSecs) { - usleep(m_secs * 1000); + usleep(mSecs*1000); } @@ -180,11 +182,13 @@ unsigned long inv_get_tick_count() { struct timeval tv; - if (gettimeofday(&tv, NULL) != 0) + if (gettimeofday(&tv, NULL) !=0) return 0; return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); } -/** @} */ + /**********************/ + /** @} */ /* defgroup */ +/**********************/ diff --git a/libsensors_iio/software/core/mllite/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 @@ double inv_vector_norm(const float *x) { 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 @@ extern "C" { 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 @@ struct results_t { long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ int acc_state; /**< Describes accel state */ - int got_accel_bias; /**< Flag describing if accel bias is known */ long compass_bias_error[3]; /**< Error Squared */ unsigned char motion_state; unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ @@ -322,24 +318,6 @@ inv_error_t inv_enable_results_holder() return result; } -/** Sets state of if we know the accel bias. - * @return return 1 if we know the accel bias, 0 if not. - * it is set with inv_set_accel_bias_found() - */ -int inv_got_accel_bias() -{ - return rh.got_accel_bias; -} - -/** Sets whether we know the accel bias - * @param[in] state Set to 1 if we know the accel bias. - * Can be retrieved with inv_got_accel_bias() - */ -void inv_set_accel_bias_found(int state) -{ - rh.got_accel_bias = state; -} - /** Sets state of if we know the compass bias. * @return return 1 if we know the compass bias, 0 if not. * it is set with inv_set_compass_bias_found() diff --git a/libsensors_iio/software/core/mllite/results_holder.h b/libsensors_iio/software/core/mllite/results_holder.h index 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 @@ inv_error_t inv_get_linear_accel_float(float *data); void inv_set_heading_confidence_interval(float ci); float inv_get_heading_confidence_interval(void); -int inv_got_accel_bias(); -void inv_set_accel_bias_found(int state); - - #ifdef __cplusplus } #endif diff --git a/libsensors_iio/software/core/mllite/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 Binary files differindex 5e74a60..116b618 100644 --- a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so +++ b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so 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 @@ 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 @@ -44,6 +43,11 @@ LLINK += -ldl LFLAGS += $(CMDLINE_LFLAGS) LFLAGS += -shared LFLAGS += -Wl,-soname,$(LIBRARY) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive LFLAGS += -Wl,-shared,-Bsymbolic LFLAGS += $(ANDROID_LINK) LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib diff --git a/libsensors_iio/software/core/mpl/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 @@ extern "C" { 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 @@ extern "C" { 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 Binary files differnew file mode 100644 index 0000000..228abf7 --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk index cd79cfa..b1d881c 100644 --- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk +++ b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk @@ -1,4 +1,4 @@ -EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX) +EXEC = consoledmp$(SHARED_APP_SUFFIX) MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) @@ -8,15 +8,16 @@ LINK ?= $(CROSS)gcc 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 CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) CFLAGS += -Wall CFLAGS += -fpic CFLAGS += -nostdlib @@ -48,8 +49,20 @@ LLINK += -lstdc++ LLINK += -llog LLINK += -lz +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += $(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 @@ -74,7 +87,7 @@ all: $(EXEC) $(MK_NAME) $(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") - $(LINK) $(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 Binary files differdeleted file mode 100644 index ede049d..0000000 --- a/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared +++ /dev/null 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 Binary files differnew file mode 100644 index 0000000..5d52b21 --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared 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 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 @@ OBJFOLDER = $(CURDIR)/obj 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 += -lstdc++ LLINK += -llog LLINK += -lz +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += $(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 @@ 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) + $(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 Binary files differdeleted file mode 100644 index 14ca523..0000000 --- a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared +++ /dev/null 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 Binary files differindex 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 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 @@ HAL_DIR = $(INV_ROOT)/software/core/HAL include $(INV_ROOT)/software/build/android/common.mk CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) CFLAGS += -Wall CFLAGS += -fpic CFLAGS += -nostdlib @@ -50,8 +49,20 @@ LLINK += -lstdc++ LLINK += -llog LLINK += -lz +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += $(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 @@ 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) + $(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 @@ struct inv_db_save_t { /** 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 main(int argc, char **argv) int axis_sign = 1; unsigned char *buffer; long timestamp; - int temperature = 0; - bool compass_present = TRUE; - - result = inv_init_sysfs_attributes(); - if (result) - return -1; + int temperature=0; + // 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 + if (fptr != NULL) { + // 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"); } - } 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"); + 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 { - 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; - } + memset(accel_bias,0, sizeof(accel_bias)); + printf("Self-Test:Failed Accel self-test\n"); + } - // Read temperature - fptr= fopen(mpu.temperature, "r"); - if (fptr != NULL) { - fscanf(fptr,"%d %ld", &temperature, ×tamp); - fclose(fptr); + // 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 @@ int main(int argc, char **argv) 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 @@ int main(int argc, char **argv) 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; } |