diff options
122 files changed, 29518 insertions, 4 deletions
diff --git a/6515/Android.mk b/6515/Android.mk new file mode 100644 index 0000000..b80cd55 --- /dev/null +++ b/6515/Android.mk @@ -0,0 +1,2 @@ +# libsensors_iio expects IIO drivers for an MPU6515+AK8963 +include $(call all-named-subdir-makefiles,libsensors_iio) diff --git a/6515/libsensors_iio/Android.mk b/6515/libsensors_iio/Android.mk new file mode 100644 index 0000000..f011eff --- /dev/null +++ b/6515/libsensors_iio/Android.mk @@ -0,0 +1,192 @@ +# Copyright (C) 2008 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. +# Modified 2011 by InvenSense, Inc + +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\" + +# ANDROID version check +MAJOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f1 -d.) +MINOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f2 -d.) +VERSION_KK :=$(shell test $(MAJOR_VERSION) -gt 4 -o $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -gt 3 && echo true) +#$(info MAJOR_VERSION=$(MAJOR_VERSION)) +#$(info MINOR_VERSION=$(MINOR_VERSION)) +#ANDROID version check END + +ifeq ($(VERSION_KK),true) +LOCAL_CFLAGS += -DANDROID_KITKAT +else +LOCAL_CFLAGS += -DANDROID_JELLYBEAN +endif + +ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug)) +ifneq ($(COMPILE_INVENSENSE_COMPASS_CAL),0) +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif +ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1) +LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL +endif +else # release builds, default +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif + +LOCAL_SRC_FILES += SensorBase.cpp +LOCAL_SRC_FILES += MPLSensor.cpp +LOCAL_SRC_FILES += MPLSupport.cpp +LOCAL_SRC_FILES += InputEventReader.cpp +LOCAL_SRC_FILES += PressureSensor.IIO.secondary.cpp + +ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug)) +ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),0) +LOCAL_SRC_FILES += AkmSensor.cpp +LOCAL_SRC_FILES += CompassSensor.AKM.cpp +else ifeq ($(COMPILE_INVENSENSE_SENSOR_ON_PRIMARY_BUS), 1) +LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp +LOCAL_CFLAGS += -DSENSOR_ON_PRIMARY_BUS +else +LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp +endif +else # release builds, default +LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp +endif #userdebug + +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 +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux + +LOCAL_SHARED_LIBRARIES := liblog +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 +LOCAL_CPPFLAGS += -DLINUX=1 +LOCAL_PRELINK_MODULE := false + +LOCAL_SHARED_LIBRARIES += libmllite +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite +LOCAL_CPPFLAGS += -DLINUX=1 +LOCAL_PRELINK_MODULE := false + +include $(BUILD_SHARED_LIBRARY) + +#endif # !TARGET_SIMULATOR + +# Build a temporary HAL that links the InvenSense .so +include $(CLEAR_VARS) +ifeq ($(filter eng, userdebug, $(TARGET_BUILD_VARIANT)),) +ifneq ($(filter manta full_grouper tilapia, $(TARGET_PRODUCT)),) +LOCAL_MODULE := sensors.full_grouper +else +ifneq ($(filter aosp_hammerhead, $(TARGET_PRODUCT)),) +LOCAL_MODULE := sensors.hammerhead +endif +ifneq ($(filter platina dory guppy, $(TARGET_PRODUCT)),) +LOCAL_MODULE := sensors.invensense +endif +endif +else # eng & userdebug builds +LOCAL_MODULE := sensors.invensense +endif # eng & userdebug builds +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_KK),true) +LOCAL_CFLAGS += -DANDROID_KITKAT +else +LOCAL_CFLAGS += -DANDROID_JELLYBEAN +endif + +ifneq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug)) +ifneq ($(COMPILE_INVENSENSE_COMPASS_CAL),0) +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif +ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1) +LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL +endif +ifeq ($(COMPILE_INVENSENSE_SENSOR_ON_PRIMARY_BUS), 1) +LOCAL_SRC_FILES += CompassSensor.IIO.primary.cpp +LOCAL_CFLAGS += -DSENSOR_ON_PRIMARY_BUS +else +LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp +endif +else # release builds, default +LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp +endif # userdebug + +ifeq (,$(filter $(TARGET_BUILD_VARIANT),eng userdebug)) +ifneq ($(filter manta grouper tilapia, $(TARGET_DEVICE)),) +# it's already defined in some other Makefile for production builds +#LOCAL_SRC_FILES := sensors_mpl.cpp +else +LOCAL_SRC_FILES := sensors_mpl.cpp +endif +else # eng & userdebug builds +LOCAL_SRC_FILES := sensors_mpl.cpp +endif # eng & userdebug builds + +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 +OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) +include $(BUILD_PREBUILT) + +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) + diff --git a/6515/libsensors_iio/CompassSensor.AKM.cpp b/6515/libsensors_iio/CompassSensor.AKM.cpp new file mode 100644 index 0000000..45699fe --- /dev/null +++ b/6515/libsensors_iio/CompassSensor.AKM.cpp @@ -0,0 +1,203 @@ +/* + * 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. + */ + +#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 "sensor_params.h" +#include "MPLSupport.h" + +// TODO: include corresponding header file for 3rd party compass sensor +#include "CompassSensor.AKM.h" + +// TODO: specify this for "fillList()" API +#define COMPASS_NAME "AKM8963" + +/*****************************************************************************/ + +CompassSensor::CompassSensor() + : SensorBase(NULL, NULL) +{ + VFUNC_LOG; + + // TODO: initiate 3rd-party's class, and disable its funtionalities + // proper commands + mCompassSensor = new AkmSensor(); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, "/sys/class/compass/akm8963/enable_mag", getTimestamp()); + write_sysfs_int((char*)"/sys/class/compass/akm8963/enable_mag", 0); +} + +CompassSensor::~CompassSensor() +{ + VFUNC_LOG; + + // TODO: disable 3rd-party's funtionalities and delete the object + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, "/sys/class/compass/akm8963/enable_mag", getTimestamp()); + write_sysfs_int((char*)"/sys/class/compass/akm8963/enable_mag", 0); + delete mCompassSensor; +} + +int CompassSensor::getFd(void) const +{ + VFUNC_LOG; + + // TODO: return 3rd-party's file descriptor + return mCompassSensor->getFd(); +} + +/** + * @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; + + // TODO: called 3rd-party's "set enable/disable" function + return mCompassSensor->setEnable(handle, en); +} + +int CompassSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + + // TODO: called 3rd-party's "set delay" function + return mCompassSensor->setDelay(handle, ns); +} + +/** + @brief This function will return the state of the sensor. + @return 1=enabled; 0=disabled +**/ +int CompassSensor::getEnable(int32_t handle) +{ + VFUNC_LOG; + + // TODO: return if 3rd-party compass is enabled + return mCompassSensor->getEnable(handle); +} + +/** + @brief This function will return the current delay for this sensor. + @return delay in nanoseconds. +**/ +int64_t CompassSensor::getDelay(int32_t handle) +{ + VFUNC_LOG; + + // TODO: return 3rd-party's delay time (should be in ns) + return mCompassSensor->getDelay(handle); +} + +/** + @brief Integrators need to implement this function per 3rd-party solution + @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; + + // TODO: need to implement "readSample()" for MPL in 3rd-party's .cpp file + return mCompassSensor->readSample(data, timestamp); +} + +/** + @brief Integrators need to implement this function per 3rd-party solution + @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::readRawSample(float *data, int64_t *timestamp) +{ + VFUNC_LOG; + long ldata[3]; + + int res = mCompassSensor->readSample(ldata, timestamp); + for(int i=0; i<3; i++) { + data[i] = (float)ldata[i]; + } + return res; +} + +void CompassSensor::fillList(struct sensor_t *list) +{ + VFUNC_LOG; + + const char *compass = COMPASS_NAME; + + if (compass) { + if (!strcmp(compass, "AKM8963")) { + list->maxRange = COMPASS_AKM8963_RANGE; + list->resolution = COMPASS_AKM8963_RESOLUTION; + list->power = COMPASS_AKM8963_POWER; + list->minDelay = COMPASS_AKM8963_MINDELAY; + return; + } + if (!strcmp(compass, "AKM8975")) { + list->maxRange = COMPASS_AKM8975_RANGE; + list->resolution = COMPASS_AKM8975_RESOLUTION; + list->power = COMPASS_AKM8975_POWER; + list->minDelay = COMPASS_AKM8975_MINDELAY; + LOGW("HAL:support for AKM8975 is incomplete"); + } + } + + LOGE("HAL:unsupported compass id %s -- " + "this implementation only supports AKM compasses", compass); + list->maxRange = COMPASS_AKM8975_RANGE; + list->resolution = COMPASS_AKM8975_RESOLUTION; + list->power = COMPASS_AKM8975_POWER; + list->minDelay = COMPASS_AKM8975_MINDELAY; +} + +// TODO: specify compass sensor's mounting matrix for MPL +void CompassSensor::getOrientationMatrix(signed char *orient) +{ + VFUNC_LOG; + + orient[0] = 1; + orient[1] = 0; + orient[2] = 0; + orient[3] = 0; + orient[4] = 1; + orient[5] = 0; + orient[6] = 0; + orient[7] = 0; + orient[8] = 1; +} + +int CompassSensor::getAccuracy(void) +{ + VFUNC_LOG; + + // TODO: need to implement "getAccuracy()" for MPL in 3rd-party's .cpp file + return mCompassSensor->getAccuracy(); +} diff --git a/6515/libsensors_iio/CompassSensor.AKM.h b/6515/libsensors_iio/CompassSensor.AKM.h new file mode 100644 index 0000000..3d215a4 --- /dev/null +++ b/6515/libsensors_iio/CompassSensor.AKM.h @@ -0,0 +1,84 @@ +/* + * 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> + +#include "SensorBase.h" + +// TODO: include 3rd-party compass vendor's header file +// p.s.: before using unified HAL, make sure 3rd-party compass +// solution's driver/HAL work well by themselves +#include "AkmSensor.h" + +/*****************************************************************************/ + +class CompassSensor : public SensorBase { + +protected: + +public: + CompassSensor(); + virtual ~CompassSensor(); + + // TODO: make sure either 3rd-party compass solution has following virtual + // functions, or SensorBase.cpp could provide equal functionalities + virtual int getFd() const; + virtual int getRawFd() {return 0;}; + 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); + virtual int64_t getMinDelay() { return -1; } // stub + + // TODO: unnecessary for MPL solution (override 3rd-party solution) + virtual int readEvents(sensors_event_t *data, int count) { return 0; } + + // TODO: following four APIs need further implementation for MPL's + // reference (look into .cpp for detailed information, also refer to + // 3rd-party's readEvents() for relevant APIs) + int readSample(long *data, int64_t *timestamp); + int readRawSample(float *data, int64_t *timestamp); + void fillList(struct sensor_t *list); + void getOrientationMatrix(signed char *orient); + int getAccuracy(); + virtual void getCompassBias(long *bias) {return;}; + + // TODO: if 3rd-party provides calibrated compass data, just return 1 + int providesCalibration() { return 1; } + + // TODO: hard-coded for 3rd-party's sensitivity transformation + long getSensitivity() { return (1L << 30); } + + /* all 3rd pary solution have compasses on the primary bus, hence they + have no dependency on the MPU */ + int isIntegrated() { return 0; } + + int checkCoilsReset(void) { return 0; }; + int isYasCompass(void) { return 0; }; + +private: + AkmSensor *mCompassSensor; +}; + +/*****************************************************************************/ + +#endif // COMPASS_SENSOR_H diff --git a/6515/libsensors_iio/CompassSensor.IIO.9150.cpp b/6515/libsensors_iio/CompassSensor.IIO.9150.cpp new file mode 100644 index 0000000..cc9d38c --- /dev/null +++ b/6515/libsensors_iio/CompassSensor.IIO.9150.cpp @@ -0,0 +1,416 @@ +/* + * 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.9150.h" +#include "sensors.h" +#include "MPLSupport.h" +#include "sensor_params.h" +#include "ml_sysfs_helper.h" + +#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) +#define COMPASS_NAME "USE_SYSFS" + +#if defined COMPASS_YAS53x +#pragma message("HAL:build Invensense compass cal with YAS53x IIO on secondary bus") +#define USE_MPL_COMPASS_HAL (1) +#define COMPASS_NAME "INV_YAS530" + +#elif defined COMPASS_AK8975 +#pragma message("HAL:build Invensense compass cal with AK8975 on primary bus") +#define USE_MPL_COMPASS_HAL (1) +#define COMPASS_NAME "INV_AK8975" + +#elif defined INVENSENSE_COMPASS_CAL +# define COMPASS_NAME "USE_SYSFS" +#pragma message("HAL:build Invensense compass cal with compass IIO on secondary bus") +#define USE_MPL_COMPASS_HAL (1) +#else +#pragma message("HAL:build 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(NULL, NULL), + compass_fd(-1), + mCompassTimestamp(0), + mCompassInputReader(8) +{ + VFUNC_LOG; + + if(!strcmp(COMPASS_NAME, "USE_SYSFS")) { + int result = find_name_by_sensor_type("in_magn_scale", "iio:device", + sensor_name); + if(result) { + LOGE("HAL:Cannot read secondary device name - (%d)", result); + } + dev_name = sensor_name; + } + LOGI_IF(PROCESS_VERBOSE, "HAL:Secondary Chip Id: %s", dev_name); + + if(inv_init_sysfs_attributes()) { + LOGE("Error Instantiating Compass\n"); + return; + } + + memset(mCachedCompassData, 0, sizeof(mCachedCompassData)); + + 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]; + if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { + LOGE("HAL:Could not read compass mounting matrix"); + } else { + 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]; + } + } + + if (!isIntegrated()) { + enable(ID_M, 0); + } +} + +CompassSensor::~CompassSensor() +{ + VFUNC_LOG; + free(pathP); + if( compass_fd > 0) + close(compass_fd); +} + +int CompassSensor::getFd() const +{ + VFUNC_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; + int res = 0; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_enable, getTimestamp()); + res = write_sysfs_int(compassSysFs.compass_enable, 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; +} + +int CompassSensor::turnOffCompassFifo(void) +{ + int i, res = 0, tempFd; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, compassSysFs.compass_fifo_enable, getTimestamp()); + res += write_sysfs_int(compassSysFs.compass_fifo_enable, 0); + return res; +} + +int CompassSensor::turnOnCompassFifo(void) +{ + int i, res = 0, tempFd; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, compassSysFs.compass_fifo_enable, getTimestamp()); + res += write_sysfs_int(compassSysFs.compass_fifo_enable, 1); + 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) +{ + VHANDLER_LOG; + + int numEventReceived = 0, done = 0; + + ssize_t n = mCompassInputReader.fill(compass_fd); + if (n < 0) { + LOGE("HAL:no compass events read"); + return n; + } + + input_event const* event; + + while (done == 0 && mCompassInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_REL) { + processCompassEvent(event); + } else if (type == EV_SYN) { + *timestamp = mCompassTimestamp; + memcpy(data, mCachedCompassData, sizeof(mCachedCompassData)); + done = 1; + } else { + LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)", + type, event->code); + } + mCompassInputReader.next(); + } + + return done; +} + +/** + * @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 = sensor_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") + || !strcmp(compass, "AK8975") + || !strcmp(compass, "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, "compass") + || !strcmp(compass, "INV_AK8963") + || !strcmp(compass, "AK8963") + || !strcmp(compass, "ak8963")) { + list->maxRange = COMPASS_AKM8963_RANGE; + list->resolution = COMPASS_AKM8963_RESOLUTION; + list->power = COMPASS_AKM8963_POWER; + list->minDelay = COMPASS_AKM8963_MINDELAY; + return; + } + if(!strcmp(compass, "compass") + || !strncmp(compass, "mlx90399",3) + || !strncmp(compass, "MLX90399",3)) { + list->maxRange = COMPASS_MPU9350_RANGE; + list->resolution = COMPASS_MPU9350_RESOLUTION; + list->power = COMPASS_MPU9350_POWER; + list->minDelay = COMPASS_MPU9350_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; + list->power = COMPASS_AMI306_POWER; + list->minDelay = COMPASS_AMI306_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; +} + +int CompassSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + unsigned char i = 0; + char sysfs_path[MAX_SYSFS_NAME_LEN]; + char iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2]; + char *sptr; + char **dptr; + int num; + + pathP = (char*)malloc( + sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + sptr = pathP; + dptr = (char**)&compassSysFs; + if (sptr == NULL) + return -1; + + memset(sysfs_path, 0, sizeof(sysfs_path)); + memset(iio_trigger_path, 0, sizeof(iio_trigger_path)); + + do { + *dptr++ = sptr; + memset(sptr, 0, sizeof(sptr)); + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < COMPASS_MAX_SYSFS_ATTRB); + + // 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); + +#if defined COMPASS_AK8975 + inv_get_input_number(dev_name, &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 + sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable"); + sprintf(compassSysFs.compass_fifo_enable, "%s%s", sysfs_path, "/compass_fifo_enable"); + sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/compass_rate"); + sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale"); + sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); +#endif + +#if 0 + // test print sysfs paths + dptr = (char**)&compassSysFs; + for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} diff --git a/6515/libsensors_iio/CompassSensor.IIO.9150.h b/6515/libsensors_iio/CompassSensor.IIO.9150.h new file mode 100644 index 0000000..f981cd7 --- /dev/null +++ b/6515/libsensors_iio/CompassSensor.IIO.9150.h @@ -0,0 +1,100 @@ +/* + * 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" + +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); + virtual int64_t getMinDelay() { return -1; } // stub + + // unnecessary for MPL + virtual int readEvents(sensors_event_t *data, int count) { return 0; } + + int turnOffCompassFifo(void); + int turnOnCompassFifo(void); + 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 (1); } + int isYasCompass(void) { return (0); } + int checkCoilsReset(void) { return(-1); } + +private: + char sensor_name[200]; + enum CompassBus { + COMPASS_BUS_PRIMARY = 0, + COMPASS_BUS_SECONDARY = 1 + } mI2CBus; + + struct sysfs_attrbs { + char *compass_enable; + char *compass_fifo_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; + } compassSysFs; + + // implementation specific + signed char mCompassOrientation[9]; + long mCachedCompassData[3]; + int compass_fd; + int64_t mCompassTimestamp; + InputEventCircularReader mCompassInputReader; + int64_t mDelay; + int mEnable; + char *pathP; + + void processCompassEvent(const input_event *event); + int inv_init_sysfs_attributes(void); +}; + +/*****************************************************************************/ + +#endif // COMPASS_SENSOR_H diff --git a/6515/libsensors_iio/CompassSensor.IIO.primary.cpp b/6515/libsensors_iio/CompassSensor.IIO.primary.cpp new file mode 100644 index 0000000..5d8d120 --- /dev/null +++ b/6515/libsensors_iio/CompassSensor.IIO.primary.cpp @@ -0,0 +1,563 @@ +/* + * 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" + +#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) +#define COMPASS_NAME "USE_SYSFS" + +#if defined COMPASS_AK8975 +#pragma message("HAL:build Invensense compass cal with AK8975 on primary bus") +#define USE_MPL_COMPASS_HAL (1) +#define COMPASS_NAME "INV_AK8975" +#endif + +/******************************************************************************/ + +CompassSensor::CompassSensor() + : SensorBase(COMPASS_NAME, NULL), + mCompassTimestamp(0), + mCompassInputReader(8), + mCoilsResetFd(0) +{ + FILE *fptr; + + VFUNC_LOG; + + mYasCompass = false; + if(!strcmp(dev_name, "USE_SYSFS")) { + char sensor_name[20]; + find_name_by_sensor_type("in_magn_x_raw", "iio:device", sensor_name); + strncpy(dev_full_name, sensor_name, + sizeof(dev_full_name) / sizeof(dev_full_name[0])); + if(!strncmp(dev_full_name, "yas", 3)) { + mYasCompass = true; + } + } else { + +#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 { + mYasCompass = true; + } +#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]; + if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]) < 0 || fclose(fptr)) { + LOGE("HAL:could not read compass mounting matrix"); + } else { + + 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]; + } + } + + if(mYasCompass) { + mCoilsResetFd = fopen(compassSysFs.compass_attr_1, "r+"); + if (fptr == NULL) { + LOGE("HAL:Could not open compass overunderflow"); + } + } +} + +void CompassSensor::enable_iio_sysfs() +{ + VFUNC_LOG; + + int tempFd = 0; + char iio_device_node[MAX_CHIP_ID_LEN]; + FILE *tempFp = NULL; + const char* compass = dev_full_name; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, compassSysFs.in_timestamp_en, getTimestamp()); + write_sysfs_int(compassSysFs.in_timestamp_en, 1); + + 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 || fclose(tempFp) < 0) { + LOGE("HAL:could not write buffer length"); + } + } + + 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); + if(mYasCompass) { + if( mCoilsResetFd != NULL ) + fclose(mCoilsResetFd); + } +} + +int CompassSensor::getFd(void) const +{ + VHANDLER_LOG; + LOGI_IF(0, "HAL:compass_fd=%d", compass_fd); + 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 = 0; + + /* 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()); + res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_y_fifo_enable, getTimestamp()); + res += write_sysfs_int(compassSysFs.compass_y_fifo_enable, en); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_z_fifo_enable, getTimestamp()); + res += write_sysfs_int(compassSysFs.compass_z_fifo_enable, en); + + res = masterEnable(en); + if (res < en) { + return res; + } + } + + return res; +} + +int CompassSensor::masterEnable(int en) +{ + VFUNC_LOG; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.chip_enable, getTimestamp()); + return write_sysfs_int(compassSysFs.chip_enable, en); +} + +int CompassSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + int tempFd; + int res; + + mDelay = ns; + if (ns == 0) + return -1; + tempFd = open(compassSysFs.compass_rate, O_RDWR); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp()); + 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; + mMinDelay = list->minDelay; + return; + } + if(!strcmp(compass, "compass") + || !strcmp(compass, "INV_AK8975") + || !strcmp(compass, "AK8975") + || !strcmp(compass, "ak8975")) { + list->maxRange = COMPASS_AKM8975_RANGE; + list->resolution = COMPASS_AKM8975_RESOLUTION; + list->power = COMPASS_AKM8975_POWER; + list->minDelay = COMPASS_AKM8975_MINDELAY; + mMinDelay = list->minDelay; + return; + } + if(!strcmp(compass, "compass") + || !strcmp(compass, "INV_AK8963") + || !strcmp(compass, "AK8963") + || !strcmp(compass, "ak8963")) { + list->maxRange = COMPASS_AKM8963_RANGE; + list->resolution = COMPASS_AKM8963_RESOLUTION; + list->power = COMPASS_AKM8963_POWER; + list->minDelay = COMPASS_AKM8963_MINDELAY; + mMinDelay = list->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; + mMinDelay = list->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; + mMinDelay = list->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; + mMinDelay = list->minDelay; +} + +/* 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; +} + +int CompassSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + unsigned char i = 0; + char sysfs_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")); + +#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.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(mYasCompass) { + sprintf(compassSysFs.compass_attr_1, "%s%s", sysfs_path, "/overunderflow"); + } +#endif + +#if 0 + // test print sysfs paths + dptr = (char**)&compassSysFs; + LOGI("sysfs path base: %s", sysfs_path); + for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} + +int CompassSensor::isYasCompass(void) +{ + return mYasCompass; +} diff --git a/6515/libsensors_iio/CompassSensor.IIO.primary.h b/6515/libsensors_iio/CompassSensor.IIO.primary.h new file mode 100644 index 0000000..de64799 --- /dev/null +++ b/6515/libsensors_iio/CompassSensor.IIO.primary.h @@ -0,0 +1,114 @@ +/* + * 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) +#define COMPASS_ON_PRIMARY "in_magn_x_raw" + +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); + virtual int64_t getMinDelay() { return mMinDelay; } + + // unnecessary for MPL + virtual int readEvents(sensors_event_t *data, int count) { return 0; } + + int readSample(long *data, int64_t *timestamp); + int readRawSample(float *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 (0); } + int checkCoilsReset(void); + int isYasCompass(void); + +private: + enum CompassBus { + COMPASS_BUS_PRIMARY = 0, + COMPASS_BUS_SECONDARY = 1 + } mI2CBus; + + struct sysfs_attrbs { + char *chip_enable; + char *in_timestamp_en; + 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; + char *compass_attr_1; + } 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; + int64_t mMinDelay; + 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); + FILE *mCoilsResetFd; + bool mYasCompass; +}; + +/*****************************************************************************/ + +#endif // COMPASS_SENSOR_H diff --git a/6515/libsensors_iio/InputEventReader.cpp b/6515/libsensors_iio/InputEventReader.cpp new file mode 100644 index 0000000..968e32e --- /dev/null +++ b/6515/libsensors_iio/InputEventReader.cpp @@ -0,0 +1,114 @@ +/* +* 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) +{ + mLastFd = -1; +} + +InputEventCircularReader::~InputEventCircularReader() +{ + delete [] mBuffer; +} + +#define INPUT_EVENT_DEBUG (0) +ssize_t InputEventCircularReader::fill(int fd) +{ + size_t numEventsRead = 0; + mLastFd = fd; + + 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, numEventsRead:%d\n", + __PRETTY_FUNCTION__, numEventsRead); + return numEventsRead; +} + +ssize_t InputEventCircularReader::readEvent(input_event const** events) +{ + *events = mCurr; + ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace; + LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s fd:%d, available:%d\n", + __PRETTY_FUNCTION__, mLastFd, (int)available); + return (available ? 1 : 0); +} + +void InputEventCircularReader::next() +{ + mCurr++; + mFreeSpace++; + if (mCurr >= mBufferEnd) { + mCurr = mBuffer; + } + ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace; + LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s fd:%d, still available:%d\n", + __PRETTY_FUNCTION__, mLastFd, (int)available); +} + diff --git a/6515/libsensors_iio/InputEventReader.h b/6515/libsensors_iio/InputEventReader.h new file mode 100644 index 0000000..4e33af3 --- /dev/null +++ b/6515/libsensors_iio/InputEventReader.h @@ -0,0 +1,50 @@ +/* +* 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_INPUT_EVENT_READER_H +#define ANDROID_INPUT_EVENT_READER_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + +#include "SensorBase.h" + +/*****************************************************************************/ + +struct input_event; + +class InputEventCircularReader +{ + struct input_event* const mBuffer; + struct input_event* const mBufferEnd; + struct input_event* mHead; + struct input_event* mCurr; + ssize_t mFreeSpace; + int mLastFd; + +public: + InputEventCircularReader(size_t numEvents); + ~InputEventCircularReader(); + ssize_t fill(int fd); + ssize_t readEvent(input_event const** events); + void next(); +}; + +/*****************************************************************************/ + +#endif // ANDROID_INPUT_EVENT_READER_H diff --git a/6515/libsensors_iio/License.txt b/6515/libsensors_iio/License.txt new file mode 100644 index 0000000..930f931 --- /dev/null +++ b/6515/libsensors_iio/License.txt @@ -0,0 +1,217 @@ +SOFTWARE LICENSE AGREEMENT + +Unless you and InvenSense Corporation ("InvenSense") execute a separate written +software license agreement governing use of the accompanying software, this +software is licensed to you under the terms of this Software License +Agreement ("Agreement"). + +ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR +ACCEPTANCE OF THIS AGREEMENT. + +1. DEFINITIONS. + +1.1. "InvenSense Product" means any of the proprietary integrated circuit +product(s) sold by InvenSense with which the Software was designed to be used, +or their successors. + +1.2. "Licensee" means you or if you are accepting on behalf of an entity +then the entity and its affiliates exercising rights under, and complying +with all of the terms of this Agreement. + +1.3. "Software" shall mean that software made available by InvenSense to +Licensee in binary code form with this Agreement. + +2. LICENSE GRANT; OWNERSHIP + +2.1. License Grants. Subject to the terms and conditions of this Agreement, +InvenSense hereby grants to Licensee a non-exclusive, non-transferable, +royalty-free license (i) to use and integrate the Software in conjunction +with any other software; and (ii) to reproduce and distribute the Software +complete, unmodified and only for use with a InvenSense Product. + +2.2. Restriction on Modification. If and to the extent that the Software is +designed to be compliant with any published communications standard +(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards), +Licensee may not make any modifications to the Software that would cause the +Software or the accompanying InvenSense Products to be incompatible with such +standard. + +2.3. Restriction on Distribution. Licensee shall only distribute the +Software (a) under the terms of this Agreement and a copy of this Agreement +accompanies such distribution, and (b) agrees to defend and indemnify +InvenSense and its licensors from and against any damages, costs, liabilities, +settlement amounts and/or expenses (including attorneys' fees) incurred in +connection with any claim, lawsuit or action by any third party that arises +or results from the use or distribution of any and all Software by the +Licensee except as contemplated herein. + +2.4. Proprietary Notices. Licensee shall not remove, efface or obscure any +copyright or trademark notices from the Software. Licensee shall include +reproductions of the InvenSense copyright notice with each copy of the +Software, except where such Software is embedded in a manner not readily +accessible to the end user. Licensee acknowledges that any symbols, +trademarks, tradenames, and service marks adopted by InvenSense to identify the +Software belong to InvenSense and that Licensee shall have no rights therein. + +2.5. Ownership. InvenSense shall retain all right, title and interest, +including all intellectual property rights, in and to the Software. Licensee +hereby covenants that it will not assert any claim that the Software created +by or for InvenSense infringe any intellectual property right owned or +controlled by Licensee. + +2.6. No Other Rights Granted; Restrictions. Apart from the license rights +expressly set forth in this Agreement, InvenSense does not grant and Licensee +does not receive any ownership right, title or interest nor any security +interest or other interest in any intellectual property rights relating to +the Software, nor in any copy of any part of the foregoing. No license is +granted to Licensee in any human readable code of the Software (source code). +Licensee shall not (i) use, license, sell or otherwise distribute the +Software except as provided in this Agreement, (ii) attempt to reverse +engineer, decompile or disassemble any portion of the Software; or (iii) use +the Software or other material in violation of any applicable law or +regulation, including but not limited to any regulatory agency, such as FCC, +rules. + +3. NO WARRANTY OR SUPPORT + +3.1. No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND +LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE, +COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE. INVENSENSE SPECIFICALLY +DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC +PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR +DOCUMENTATION FOR THE SOFTWARE. WITHOUT LIMITATION OF THE ABOVE, INVENSENSE +GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT +INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS +THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR +RELIABILITY. + +3.2. No Support. Nothing in this agreement shall obligate InvenSense to +provide any support for the Software. InvenSense may, but shall be under no +obligation to, correct any defects in the Software and/or provide updates to +licensees of the Software. Licensee shall make reasonable efforts to +promptly report to InvenSense any defects it finds in the Software, as an aid +to creating improved revisions of the Software. + +3.3. Dangerous Applications. The Software is not designed, intended, or +certified for use in components of systems intended for the operation of +weapons, weapons systems, nuclear installations, means of mass +transportation, aviation, life-support computers or equipment (including +resuscitation equipment and surgical implants), pollution control, hazardous +substances management, or for any other dangerous application in which the +failure of the Software could create a situation where personal injury or +death may occur. Licensee understands that use of the Software in such +applications is fully at the risk of Licensee. + +4. TERM AND TERMINATION + +4.1. Termination. This Agreement will automatically terminate if Licensee +fails to comply with any of the terms and conditions hereof. In such event, +Licensee must destroy all copies of the Software and all of its component +parts. + +4.2. Effect Of Termination. Upon any termination of this Agreement, the +rights and licenses granted to Licensee under this Agreement shall +immediately terminate. + +4.3. Survival. The rights and obligations under this Agreement which by +their nature should survive termination will remain in effect after +expiration or termination of this Agreement. + +5. CONFIDENTIALITY + +5.1. Obligations. Licensee acknowledges and agrees that any documentation +relating to the Software, and any other information (if such other +information is identified as confidential or should be recognized as +confidential under the circumstances) provided to Licensee by InvenSense +hereunder (collectively, "Confidential Information") constitute the +confidential and proprietary information of InvenSense, and that Licensee's +protection thereof is an essential condition to Licensee's use and possession +of the Software. Licensee shall retain all Confidential Information in +strict confidence and not disclose it to any third party or use it in any way +except under a written agreement with terms and conditions at least as +protective as the terms of this Section. Licensee will exercise at least the +same amount of diligence in preserving the secrecy of the Confidential +Information as it uses in preserving the secrecy of its own most valuable +confidential information, but in no event less than reasonable diligence. +Information shall not be considered Confidential Information if and to the +extent that it: (i) was in the public domain at the time it was disclosed or +has entered the public domain through no fault of Licensee; (ii) was known to +Licensee, without restriction, at the time of disclosure as proven by the +files of Licensee in existence at the time of disclosure; or (iii) becomes +known to Licensee, without restriction, from a source other than InvenSense +without breach of this Agreement by Licensee and otherwise not in violation +of InvenSense's rights. + +5.2. Return of Confidential Information. Notwithstanding the foregoing, all +documents and other tangible objects containing or representing InvenSense +Confidential Information and all copies thereof which are in the possession +of Licensee shall be and remain the property of InvenSense, and shall be +promptly returned to InvenSense upon written request by InvenSense or upon +termination of this Agreement. + +6. LIMITATION OF LIABILITY +TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF +INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL, +SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR +OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS +OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT +(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR +SOFTWARE UNDER THIS AGREEMENT. THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING +ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY. + +7. MISCELLANEOUS + +7.1. Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS +SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND +REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE +OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS. +WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE +TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED. + +7.2 Assignment. This Agreement shall be binding upon and inure to the +benefit of the parties and their respective successors and assigns, provided, +however that Licensee may not assign this Agreement or any rights or +obligation hereunder, directly or indirectly, by operation of law or +otherwise, without the prior written consent of InvenSense, and any such +attempted assignment shall be void. Notwithstanding the foregoing, Licensee +may assign this Agreement to a successor to all or substantially all of its +business or assets to which this Agreement relates that is not a competitor +of InvenSense. + +7.3. Governing Law; Venue. This Agreement shall be governed by the laws of +California without regard to any conflict-of-laws rules, and the United +Nations Convention on Contracts for the International Sale of Goods is hereby +excluded. The sole jurisdiction and venue for actions related to the subject +matter hereof shall be the state and federal courts located in the County of +Orange, California, and both parties hereby consent to such jurisdiction and +venue. + +7.4. Severability. All terms and provisions of this Agreement shall, if +possible, be construed in a manner which makes them valid, but in the event +any term or provision of this Agreement is found by a court of competent +jurisdiction to be illegal or unenforceable, the validity or enforceability +of the remainder of this Agreement shall not be affected if the illegal or +unenforceable provision does not materially affect the intent of this +Agreement. If the illegal or unenforceable provision materially affects the +intent of the parties to this Agreement, this Agreement shall become +terminated. + +7.5. Equitable Relief. Licensee hereby acknowledges that its breach of this +Agreement would cause irreparable harm and significant injury to InvenSense +that may be difficult to ascertain and that a remedy at law would be +inadequate. Accordingly, Licensee agrees that InvenSense shall have the right +to seek and obtain immediate injunctive relief to enforce obligations under +the Agreement in addition to any other rights and remedies it may have. + +7.6. Waiver. The waiver of, or failure to enforce, any breach or default +hereunder shall not constitute the waiver of any other or subsequent breach +or default. + +7.7. Entire Agreement. This Agreement sets forth the entire Agreement +between the parties and supersedes any and all prior proposals, agreements +and representations between them, whether written or oral concerning the +Software. This Agreement may be changed only by mutual agreement of the +parties in writing. + diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp new file mode 100644 index 0000000..9721734 --- /dev/null +++ b/6515/libsensors_iio/MPLSensor.cpp @@ -0,0 +1,6444 @@ +/* +* 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 + +//see also the EXTRA_VERBOSE define in the MPLSensor.h header file + +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <float.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <stdlib.h> +#include <sys/select.h> +#include <sys/syscall.h> +#include <dlfcn.h> +#include <pthread.h> +#include <cutils/log.h> +#include <utils/KeyedVector.h> +#include <utils/String8.h> +#include <string.h> +#include <linux/input.h> +#include <utils/Atomic.h> + +#include "MPLSensor.h" +#include "PressureSensor.IIO.secondary.h" +#include "MPLSupport.h" +#include "sensor_params.h" + +#include "invensense.h" +#include "invensense_adv.h" +#include "ml_stored_data.h" +#include "ml_load_dmp.h" +#include "ml_sysfs_helper.h" + +#define ENABLE_MULTI_RATE +// #define TESTING +#define USE_LPQ_AT_FASTEST + +#ifdef THIRD_PARTY_ACCEL +#pragma message("HAL:build third party accel support") +#define USE_THIRD_PARTY_ACCEL (1) +#else +#define USE_THIRD_PARTY_ACCEL (0) +#endif + +#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) + +/******************************************************************************/ +/* MPL Interface */ +/******************************************************************************/ + +//#define INV_PLAYBACK_DBG +#ifdef INV_PLAYBACK_DBG +static FILE *logfile = NULL; +#endif + +/******************************************************************************* + * MPLSensor class implementation + ******************************************************************************/ + +static struct timespec mt_pre; + +// following extended initializer list would only be available with -std=c++11 +// or -std=gnu+11 +MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *)) + : SensorBase(NULL, NULL), + mMasterSensorMask(INV_ALL_SENSORS), + mLocalSensorMask(0), + mPollTime(-1), + mStepCountPollTime(-1), + mHaveGoodMpuCal(0), + mGyroAccuracy(0), + mAccelAccuracy(0), + mCompassAccuracy(0), + dmp_orient_fd(-1), + mDmpOrientationEnabled(0), + dmp_sign_motion_fd(-1), + mDmpSignificantMotionEnabled(0), + dmp_pedometer_fd(-1), + mDmpPedometerEnabled(0), + mDmpStepCountEnabled(0), + mEnabled(0), + mBatchEnabled(0), + mFlushSensorEnabled(-1), + mOldBatchEnabledMask(0), + mAccelInputReader(4), + mGyroInputReader(32), + mTempScale(0), + mTempOffset(0), + mTempCurrentTime(0), + mAccelScale(2), + mAccelSelfTestScale(2), + mGyroScale(2000), + mGyroSelfTestScale(2000), + mCompassScale(0), + mFactoryGyroBiasAvailable(false), + mGyroBiasAvailable(false), + mGyroBiasApplied(false), + mFactoryAccelBiasAvailable(false), + mAccelBiasAvailable(false), + mAccelBiasApplied(false), + mPendingMask(0), + mSensorMask(0), + mMplFeatureActiveMask(0), + mFeatureActiveMask(0), + mDmpOn(0), + mPedUpdate(0), + mPressureUpdate(0), + mQuatSensorTimestamp(0), + mStepSensorTimestamp(0), + mLastStepCount(0), + mLeftOverBufferSize(0), + mInitial6QuatValueAvailable(0), + mFlushBatchSet(0), + mSkipReadEvents(0), + mDataMarkerDetected(0), + mEmptyDataMarkerDetected(0) { + VFUNC_LOG; + + inv_error_t rv; + int i, fd; + char *port = NULL; + char *ver_str; + unsigned long mSensorMask; + int res; + FILE *fptr; + + mCompassSensor = compass; + + LOGV_IF(EXTRA_VERBOSE, + "HAL:MPLSensor constructor : NumSensors = %d", NumSensors); + + pthread_mutex_init(&mMplMutex, NULL); + pthread_mutex_init(&mHALMutex, NULL); + memset(mGyroOrientation, 0, sizeof(mGyroOrientation)); + memset(mAccelOrientation, 0, sizeof(mAccelOrientation)); + memset(mInitial6QuatValue, 0, sizeof(mInitial6QuatValue)); + + /* setup sysfs paths */ + inv_init_sysfs_attributes(); + + /* get chip name */ + if (inv_get_chip_name(chip_ID) != INV_SUCCESS) { + LOGE("HAL:ERR- Failed to get chip ID\n"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID); + } + + enable_iio_sysfs(); + + /* instantiate pressure sensor on secondary bus */ + mPressureSensor = new PressureSensor((const char*)mSysfsPath); + + /* reset driver master enable */ + masterEnable(0); + + /* Load DMP image if capable, ie. MPU6515 */ + loadDMP(); + + /* open temperature fd for temp comp */ + LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature); + gyro_temperature_fd = open(mpu.temperature, O_RDONLY); + if (gyro_temperature_fd == -1) { + LOGE("HAL:could not open temperature node"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:temperature_fd opened: %s", mpu.temperature); + } + + /* read gyro FSR to calculate accel scale later */ + char gyroBuf[5]; + int count = 0; + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp()); + + fd = open(mpu.gyro_fsr, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening gyro FSR"); + } else { + memset(gyroBuf, 0, sizeof(gyroBuf)); + count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf)); + if(count < 1) { + LOGE("HAL:Error reading gyro FSR"); + } else { + count = sscanf(gyroBuf, "%ld", &mGyroScale); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale); + } + close(fd); + } + + /* read gyro self test scale used to calculate factory cal bias later */ + char gyroScale[5]; + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp()); + fd = open(mpu.in_gyro_self_test_scale, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening gyro self test scale"); + } else { + memset(gyroBuf, 0, sizeof(gyroBuf)); + count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale)); + if(count < 1) { + LOGE("HAL:Error reading gyro self test scale"); + } else { + count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale); + } + close(fd); + } + + /* open Factory Gyro Bias fd */ + /* mFactoryGyBias contains bias values that will be used for device offset */ + memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias)); + LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset); + LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset); + LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset); + gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR); + gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR); + gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR); + if (gyro_x_offset_fd == -1 || + gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) { + LOGE("HAL:could not open factory gyro calibrated bias"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:gyro_offset opened"); + } + + /* open Gyro Bias fd */ + /* mGyroBias contains bias values that will be used for framework */ + /* mGyroChipBias contains bias values that will be used for dmp */ + memset(mGyroBias, 0, sizeof(mGyroBias)); + memset(mGyroChipBias, 0, sizeof(mGyroChipBias)); + LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias); + LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias); + LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias); + gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR); + gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR); + gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR); + if (gyro_x_dmp_bias_fd == -1 || + gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) { + LOGE("HAL:could not open gyro DMP calibrated bias"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:gyro_dmp_bias opened"); + } + + /* read accel FSR to calcuate accel scale later */ + if (USE_THIRD_PARTY_ACCEL == 0) { + char buf[3]; + int count = 0; + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp()); + + fd = open(mpu.accel_fsr, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening accel FSR"); + } else { + memset(buf, 0, sizeof(buf)); + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + LOGE("HAL:Error reading accel FSR"); + } else { + count = sscanf(buf, "%d", &mAccelScale); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale); + } + close(fd); + } + + /* read accel self test scale used to calculate factory cal bias later */ + char accelScale[5]; + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.in_accel_self_test_scale, getTimestamp()); + fd = open(mpu.in_accel_self_test_scale, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening gyro self test scale"); + } else { + memset(buf, 0, sizeof(buf)); + count = read_attribute_sensor(fd, accelScale, sizeof(accelScale)); + if(count < 1) { + LOGE("HAL:Error reading accel self test scale"); + } else { + count = sscanf(accelScale, "%ld", &mAccelSelfTestScale); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Accel self test scale used %ld", mAccelSelfTestScale); + } + close(fd); + } + + /* open Factory Accel Bias fd */ + /* mFactoryAccelBias contains bias values that will be used for device offset */ + memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias)); + LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel x offset path: %s", mpu.in_accel_x_offset); + LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel y offset path: %s", mpu.in_accel_y_offset); + LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel z offset path: %s", mpu.in_accel_z_offset); + accel_x_offset_fd = open(mpu.in_accel_x_offset, O_RDWR); + accel_y_offset_fd = open(mpu.in_accel_y_offset, O_RDWR); + accel_z_offset_fd = open(mpu.in_accel_z_offset, O_RDWR); + if (accel_x_offset_fd == -1 || + accel_y_offset_fd == -1 || accel_z_offset_fd == -1) { + LOGE("HAL:could not open factory accel calibrated bias"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:accel_offset opened"); + } + + /* open Accel Bias fd */ + /* mAccelBias contains bias that will be used for dmp */ + memset(mAccelBias, 0, sizeof(mAccelBias)); + LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias); + LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias); + LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias); + accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR); + accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR); + accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR); + if (accel_x_dmp_bias_fd == -1 || + accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) { + LOGE("HAL:could not open accel DMP calibrated bias"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:accel_dmp_bias opened"); + } + } + + dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK); + if (dmp_sign_motion_fd < 0) { + LOGE("HAL:ERR couldn't open dmp_sign_motion node"); + } else { + LOGV_IF(ENG_VERBOSE, + "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd); + } +#if 1 + /* the following threshold can be modified for SMD sensitivity */ + int motionThreshold = 3000; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + motionThreshold, mpu.smd_threshold, getTimestamp()); + res = write_sysfs_int(mpu.smd_threshold, motionThreshold); +#endif + dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK); + if (dmp_pedometer_fd < 0) { + LOGE("HAL:ERR couldn't open dmp_pedometer node"); + } else { + LOGV_IF(ENG_VERBOSE, + "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd); + } + + initBias(); + + (void)inv_get_version(&ver_str); + LOGI("%s\n", ver_str); + + /* setup MPL */ + inv_constructor_init(); + +#ifdef INV_PLAYBACK_DBG + LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); + logfile = fopen("/data/playback.bin", "w+"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif + + /* setup orientation matrix and scale */ + inv_set_device_properties(); + + /* initialize sensor data */ + memset(mPendingEvents, 0, sizeof(mPendingEvents)); + + mPendingEvents[RotationVector].version = sizeof(sensors_event_t); + mPendingEvents[RotationVector].sensor = ID_RV; + mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; + mPendingEvents[RotationVector].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t); + mPendingEvents[GameRotationVector].sensor = ID_GRV; + mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR; + mPendingEvents[GameRotationVector].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); + mPendingEvents[LinearAccel].sensor = ID_LA; + mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; + mPendingEvents[LinearAccel].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Gravity].version = sizeof(sensors_event_t); + mPendingEvents[Gravity].sensor = ID_GR; + mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; + mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Gyro].version = sizeof(sensors_event_t); + mPendingEvents[Gyro].sensor = ID_GY; + mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; + mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[RawGyro].version = sizeof(sensors_event_t); + mPendingEvents[RawGyro].sensor = ID_RG; + mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; + mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); + mPendingEvents[Accelerometer].sensor = ID_A; + mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; + mPendingEvents[Accelerometer].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + /* Invensense compass calibration */ + mPendingEvents[MagneticField].version = sizeof(sensors_event_t); + mPendingEvents[MagneticField].sensor = ID_M; + mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; + mPendingEvents[MagneticField].magnetic.status = + SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t); + mPendingEvents[RawMagneticField].sensor = ID_RM; + mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; + mPendingEvents[RawMagneticField].magnetic.status = + SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Pressure].version = sizeof(sensors_event_t); + mPendingEvents[Pressure].sensor = ID_PS; + mPendingEvents[Pressure].type = SENSOR_TYPE_PRESSURE; + mPendingEvents[Pressure].magnetic.status = + SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Orientation].version = sizeof(sensors_event_t); + mPendingEvents[Orientation].sensor = ID_O; + mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; + mPendingEvents[Orientation].orientation.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t); + mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV; + mPendingEvents[GeomagneticRotationVector].type + = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR; + mPendingEvents[GeomagneticRotationVector].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mSmEvents.version = sizeof(sensors_event_t); + mSmEvents.sensor = ID_SM; + mSmEvents.type = SENSOR_TYPE_SIGNIFICANT_MOTION; + mSmEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE; + + mSdEvents.version = sizeof(sensors_event_t); + mSdEvents.sensor = ID_P; + mSdEvents.type = SENSOR_TYPE_STEP_DETECTOR; + mSdEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE; + + mScEvents.version = sizeof(sensors_event_t); + mScEvents.sensor = ID_SC; + mScEvents.type = SENSOR_TYPE_STEP_COUNTER; + mScEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE; + + /* Event Handlers for HW and Virtual Sensors */ + mHandlers[RotationVector] = &MPLSensor::rvHandler; + mHandlers[GameRotationVector] = &MPLSensor::grvHandler; + 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[RawMagneticField] = &MPLSensor::rawCompassHandler; + mHandlers[Orientation] = &MPLSensor::orienHandler; + mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler; + mHandlers[Pressure] = &MPLSensor::psHandler; + + /* initialize delays to reasonable values */ + for (int i = 0; i < NumSensors; i++) { + mDelays[i] = 1000000000LL; + mBatchDelays[i] = 1000000000LL; + mBatchTimeouts[i] = 100000000000LL; + } + + /* initialize Compass Bias */ + memset(mCompassBias, 0, sizeof(mCompassBias)); + + /* initialize Factory Accel Bias */ + memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias)); + + /* initialize Gyro Bias */ + memset(mGyroBias, 0, sizeof(mGyroBias)); + memset(mGyroChipBias, 0, sizeof(mGyroChipBias)); + + /* load calibration file from /data/inv_cal_data.bin */ + rv = inv_load_calibration(); + if(rv == INV_SUCCESS) { + LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded"); + /* Get initial values */ + getCompassBias(); + getGyroBias(); + if (mGyroBiasAvailable) { + setGyroBias(); + } + getAccelBias(); + getFactoryGyroBias(); + if (mFactoryGyroBiasAvailable) { + setFactoryGyroBias(); + } + getFactoryAccelBias(); + if (mFactoryAccelBiasAvailable) { + setFactoryAccelBias(); + } + } + else + LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); + + /* takes external accel calibration load workflow */ + 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_mpl_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_mask(accel_offset, mAccelAccuracy,4); + inv_get_mpl_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 external accel calibration load workflow */ + + /* disable all sensors and features */ + masterEnable(0); + enableGyro(0); + enableLowPowerAccel(0); + enableAccel(0); + enableCompass(0,0); + enablePressure(0); + enableBatch(0); + + if (isLowPowerQuatEnabled()) { + enableLPQuaternion(0); + } + + if (isDmpDisplayOrientationOn()) { + // open DMP Orient Fd + openDmpOrientFd(); + enableDmpOrientation(!isDmpScreenAutoRotationEnabled()); + } +} + +void MPLSensor::enable_iio_sysfs(void) +{ + VFUNC_LOG; + + char iio_device_node[MAX_CHIP_ID_LEN]; + FILE *tempFp = NULL; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", + mpu.in_timestamp_en, getTimestamp()); + // Either fopen()/open() are okay for sysfs access + // developers could choose what they want + // with fopen(), the benefit is that fprintf()/fscanf() are available + tempFp = fopen(mpu.in_timestamp_en, "w"); + if (tempFp == NULL) { + LOGE("HAL:could not open timestamp enable"); + } else { + if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) { + LOGE("HAL:could not enable timestamp"); + } + } + + 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 || fclose(tempFp) < 0) { + LOGE("HAL:could not write buffer length"); + } + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.chip_enable, getTimestamp()); + tempFp = fopen(mpu.chip_enable, "w"); + if (tempFp == NULL) { + LOGE("HAL:could not open chip enable"); + } else { + if (fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) { + LOGE("HAL:could not write chip enable"); + } + } + + inv_get_iio_device_node(iio_device_node); + iio_fd = open(iio_device_node, O_RDONLY); + if (iio_fd < 0) { + LOGE("HAL:could not open iio device node"); + } else { + LOGV_IF(ENG_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd); + } +} + +int MPLSensor::inv_constructor_init(void) +{ + VFUNC_LOG; + + inv_error_t result = inv_init_mpl(); + if (result) { + LOGE("HAL:inv_init_mpl() failed"); + return result; + } + result = inv_constructor_default_enable(); + result = inv_start_mpl(); + if (result) { + LOGE("HAL:inv_start_mpl() failed"); + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +int MPLSensor::inv_constructor_default_enable(void) +{ + VFUNC_LOG; + + inv_error_t result; + +/******************************************************************************* + +******************************************************************************** + +The InvenSense binary file (libmplmpu.so) is subject to Google's standard terms +and conditions as accepted in the click-through agreement required to download +this library. +The library includes, but is not limited to the following function calls: +inv_enable_quaternion(). + +ANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED. + +******************************************************************************** + +*******************************************************************************/ + + result = inv_enable_quaternion(); + if (result) { + LOGE("HAL:Cannot enable quaternion\n"); + return result; + } + + result = inv_enable_in_use_auto_calibration(); + if (result) { + return result; + } + + result = inv_enable_fast_nomot(); + if (result) { + return result; + } + + result = inv_enable_gyro_tc(); + if (result) { + return result; + } + + result = inv_enable_hal_outputs(); + if (result) { + return result; + } + + if (!mCompassSensor->providesCalibration()) { + /* Invensense compass calibration */ + LOGV_IF(ENG_VERBOSE, "HAL:Invensense vector compass cal enabled"); + result = inv_enable_vector_compass_cal(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else { + mMplFeatureActiveMask |= INV_COMPASS_CAL; + } + // specify MPL's trust weight, used by compass algorithms + inv_vector_compass_cal_sensitivity(3); + + /* disabled by default + result = inv_enable_compass_bias_w_gyro(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + */ + + result = inv_enable_heading_from_gyro(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_enable_magnetic_disturbance(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + //inv_enable_magnetic_disturbance_logging(); + } + + result = inv_enable_9x_sensor_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else { + // 9x sensor fusion enables Compass fit + mMplFeatureActiveMask |= INV_COMPASS_FIT; + } + + result = inv_enable_no_gyro_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/* TODO: create function pointers to calculate scale */ +void MPLSensor::inv_set_device_properties(void) +{ + VFUNC_LOG; + + unsigned short orient; + + inv_get_sensors_orientation(); + + inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE); + inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE); + + /* gyro setup */ + orient = inv_orientation_matrix_to_scalar(mGyroOrientation); + inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15); + LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15); + + /* accel setup */ + orient = inv_orientation_matrix_to_scalar(mAccelOrientation); + /* use for third party accel input subsystem driver + inv_set_accel_orientation_and_scale(orient, 1LL << 22); + */ + inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15); + LOGI_IF(EXTRA_VERBOSE, + "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15); + + /* compass setup */ + signed char orientMtx[9]; + mCompassSensor->getOrientationMatrix(orientMtx); + orient = + inv_orientation_matrix_to_scalar(orientMtx); + long sensitivity; + sensitivity = mCompassSensor->getSensitivity(); + inv_set_compass_orientation_and_scale(orient, sensitivity); + mCompassScale = sensitivity; + LOGI_IF(EXTRA_VERBOSE, + "HAL: Set MPL Compass Scale %ld", mCompassScale); +} + +void MPLSensor::loadDMP(void) +{ + VFUNC_LOG; + + int res, fd; + FILE *fptr; + + if (isMpuNonDmp()) { + return; + } + + /* load DMP firmware */ + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp()); + fd = open(mpu.firmware_loaded, O_RDONLY); + if(fd < 0) { + LOGE("HAL:could not open dmp state"); + } else { + if(inv_read_dmp_state(fd) == 0) { + LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware); + fptr = fopen(mpu.dmp_firmware, "w"); + if(fptr == NULL) { + LOGE("HAL:could not open dmp_firmware"); + } else { + if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) { + LOGE("HAL:load DMP failed"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); + } + } + } else { + LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded"); + } + } + + // onDmp(1); //Can't enable here. See note onDmp() +} + +void MPLSensor::inv_get_sensors_orientation(void) +{ + VFUNC_LOG; + + FILE *fptr; + + // get gyro orientation + 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]; + if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { + LOGE("HAL:Could not read gyro mounting matrix"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:gyro 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]); + + mGyroOrientation[0] = om[0]; + mGyroOrientation[1] = om[1]; + mGyroOrientation[2] = om[2]; + mGyroOrientation[3] = om[3]; + mGyroOrientation[4] = om[4]; + mGyroOrientation[5] = om[5]; + mGyroOrientation[6] = om[6]; + mGyroOrientation[7] = om[7]; + mGyroOrientation[8] = om[8]; + } + } + + // get accel orientation + 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]; + if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { + LOGE("HAL:could not read accel mounting matrix"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:accel 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]); + + mAccelOrientation[0] = om[0]; + mAccelOrientation[1] = om[1]; + mAccelOrientation[2] = om[2]; + mAccelOrientation[3] = om[3]; + mAccelOrientation[4] = om[4]; + mAccelOrientation[5] = om[5]; + mAccelOrientation[6] = om[6]; + mAccelOrientation[7] = om[7]; + mAccelOrientation[8] = om[8]; + } + } +} + +MPLSensor::~MPLSensor() +{ + VFUNC_LOG; + + /* Close open fds */ + if (iio_fd > 0) + close(iio_fd); + if( accel_fd > 0 ) + close(accel_fd ); + if (gyro_temperature_fd > 0) + close(gyro_temperature_fd); + if (sysfs_names_ptr) + free(sysfs_names_ptr); + + closeDmpOrientFd(); + + if (accel_x_dmp_bias_fd > 0) { + close(accel_x_dmp_bias_fd); + } + if (accel_y_dmp_bias_fd > 0) { + close(accel_y_dmp_bias_fd); + } + if (accel_z_dmp_bias_fd > 0) { + close(accel_z_dmp_bias_fd); + } + + if (gyro_x_dmp_bias_fd > 0) { + close(gyro_x_dmp_bias_fd); + } + if (gyro_y_dmp_bias_fd > 0) { + close(gyro_y_dmp_bias_fd); + } + if (gyro_z_dmp_bias_fd > 0) { + close(gyro_z_dmp_bias_fd); + } + + if (gyro_x_offset_fd > 0) { + close(gyro_x_dmp_bias_fd); + } + if (gyro_y_offset_fd > 0) { + close(gyro_y_offset_fd); + } + if (gyro_z_offset_fd > 0) { + close(accel_z_offset_fd); + } + + /* Turn off Gyro master enable */ + /* A workaround until driver handles it */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.master_enable, getTimestamp()); + write_sysfs_int(mpu.master_enable, 0); + +#ifdef INV_PLAYBACK_DBG + inv_turn_off_data_logging(); + if (fclose(logfile) < 0) { + LOGE("cannot close debug log file"); + } +#endif +} + +#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) +#define RGY_ENABLED ((1 << ID_RG) & enabled_sensors) +#define A_ENABLED ((1 << ID_A) & enabled_sensors) +#define M_ENABLED ((1 << ID_M) & enabled_sensors) +#define RM_ENABLED ((1 << ID_RM) & enabled_sensors) +#define PS_ENABLED ((1 << ID_PS) & 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) +#define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors) +#define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors) + +/* this applies to BMA250 Input Subsystem Driver only */ +int MPLSensor::setAccelInitialState() +{ + VFUNC_LOG; + + struct input_absinfo absinfo_x; + struct input_absinfo absinfo_y; + struct input_absinfo absinfo_z; + float value; + if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) && + !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) && + !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) { + value = absinfo_x.value; + mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X; + value = absinfo_y.value; + mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y; + value = absinfo_z.value; + mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z; + //mHasPendingEvent = true; + } + return 0; +} + +int MPLSensor::onDmp(int en) +{ + VFUNC_LOG; + + int res = -1; + int status; + mDmpOn = en; + + //Sequence to enable DMP + //1. Load DMP image if not already loaded + //2. Either Gyro or Accel must be enabled/configured before next step + //3. 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"); + } else { + mDmpOn = en; + res = 0; //Indicate write successful + if(!en) { + setAccelBias(); + } + } + //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"); + } + + // disable DMP event interrupt if needed + if (!en) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + } + } else { + mDmpOn = en; + res = 0; //DMP already set as requested + if(!en) { + setAccelBias(); + } + } + } else { + LOGE("HAL:ERR No DMP image"); + } + return res; +} + +int MPLSensor::setDmpFeature(int en) +{ + int res = 0; + + // set sensor engine and fifo + if ((mFeatureActiveMask & DMP_FEATURE_MASK) || en) { + if ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) || + (mFeatureActiveMask & INV_DMP_PED_QUATERNION) || + (mFeatureActiveMask & INV_DMP_QUATERNION)) { + res = enableGyro(1); + if (res < 0) { + return res; + } + if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) { + res = turnOffGyroFifo(); + if (res < 0) { + return res; + } + } + } + res = enableAccel(1); + if (res < 0) { + return res; + } + if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { + res = turnOffAccelFifo(); + if (res < 0) { + return res; + } + } + } else { + if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) { + res = enableGyro(0); + if (res < 0) { + return res; + } + } + if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { + res = enableAccel(0); + if (res < 0) { + return res; + } + } + } + + // set sensor data interrupt + uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE)); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + !dataInterrupt, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + return res; +} + +int MPLSensor::computeAndSetDmpState() +{ + int res = 0; + bool dmpState = 0; + + if (mFeatureActiveMask) { + dmpState = 1; + LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() mFeatureActiveMask = 1"); + } else if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK) + || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) { + if (checkLPQuaternion() && checkLPQRateSupported()) { + dmpState = 1; + LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() Sensor Fusion = 1"); + } + } /*else { + unsigned long sensor = mLocalSensorMask & mMasterSensorMask; + if (sensor & (INV_THREE_AXIS_ACCEL & INV_THREE_AXIS_GYRO)) { + dmpState = 1; + LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() accel and gyro = 1"); + } + }*/ + + // set Dmp state + res = onDmp(dmpState); + if (res < 0) + return res; + + if (dmpState) { + // 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 rate to 200Hz"); + return res; + } + } + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is set %s", (dmpState ? "on" : "off")); + return dmpState; +} + +/* called when batch and hw sensor enabled*/ +int MPLSensor::enablePedIndicator(int en) +{ + VFUNC_LOG; + + int res = 0; + if (en) { + if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { + //Disable DMP Pedometer Interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); + res = -1; // indicate an err + return res; + } + } + } + + LOGV_IF(ENG_VERBOSE, "HAL:Toggling step indicator to %d", en); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.step_indicator_on, getTimestamp()); + if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { + res = -1; + LOGE("HAL:ERR can't write to DMP step_indicator_on"); + } + return res; +} + +int MPLSensor::checkPedStandaloneEnabled(void) +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0); +} + +/* This feature is only used in batch mode */ +/* Stand-alone Step Detector */ +int MPLSensor::enablePedStandalone(int en) +{ + VFUNC_LOG; + + if (!en) { + enablePedStandaloneData(0); + mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE; + if (mFeatureActiveMask == 0) { + onDmp(0); + } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) { + //Re-enable DMP Pedometer Interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); + return (-1); + } + //Disable data interrupt if no continuous data + if (mEnabled == 0) { + 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) { + LOGE("HAL:ERR can't enable DMP event interrupt"); + return (-1); + } + } + } + LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone disabled"); + } else { + if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) { + LOGE("HAL:ERR can't enable Ped Standalone"); + } else { + mFeatureActiveMask |= INV_DMP_PED_STANDALONE; + //Disable DMP Pedometer Interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { + LOGE("HAL:ERR can't disable Android Pedometer Interrupt"); + return (-1); + } + //Enable 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) { + LOGE("HAL:ERR can't enable DMP event interrupt"); + return (-1); + } + LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone enabled"); + } + } + return 0; +} + +int MPLSensor:: enablePedStandaloneData(int en) +{ + VFUNC_LOG; + + int res = 0; + + // Set DMP Ped standalone + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.step_detector_on, getTimestamp()); + if (write_sysfs_int(mpu.step_detector_on, en) < 0) { + LOGE("HAL:ERR can't write DMP step_detector_on"); + res = -1; //Indicate an err + } + + // Set DMP Step indicator + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.step_indicator_on, getTimestamp()); + if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { + LOGE("HAL:ERR can't write DMP step_indicator_on"); + res = -1; //Indicate an err + } + + if (!en) { + LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped standalone"); + //Disable Accel if no sensor needs it + if (!(mFeatureActiveMask & DMP_FEATURE_MASK) + && (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_ACCEL))) { + res = enableAccel(0); + if (res < 0) + return res; + } + if (!(mFeatureActiveMask & DMP_FEATURE_MASK) + && (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_GYRO))) { + res = enableGyro(0); + if (res < 0) + return res; + } + } else { + LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone"); + // enable accel engine + res = enableAccel(1); + if (res < 0) + return res; + LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); + // disable accel FIFO + if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) { + res = turnOffAccelFifo(); + if (res < 0) + return res; + } + } + + return res; +} + +int MPLSensor::checkPedQuatEnabled(void) +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0); +} + +/* This feature is only used in batch mode */ +/* Step Detector && Game Rotation Vector */ +int MPLSensor::enablePedQuaternion(int en) +{ + VFUNC_LOG; + + if (!en) { + enablePedQuaternionData(0); + mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION; + if (mFeatureActiveMask == 0) { + onDmp(0); + } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) { + //Re-enable DMP Pedometer Interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); + return (-1); + } + //Disable data interrupt if no continuous data + if (mEnabled == 0) { + 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, en) < 0) { + LOGE("HAL:ERR can't enable DMP event interrupt"); + return (-1); + } + } + } + LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat disabled"); + } else { + if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) { + LOGE("HAL:ERR can't enable Ped Quaternion"); + } else { + mFeatureActiveMask |= INV_DMP_PED_QUATERNION; + //Disable DMP Pedometer Interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { + LOGE("HAL:ERR can't disable Android Pedometer Interrupt"); + return (-1); + } + //Enable 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) { + LOGE("HAL:ERR can't enable DMP event interrupt"); + return (-1); + } + LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat enabled"); + } + } + return 0; +} + +int MPLSensor::enablePedQuaternionData(int en) +{ + VFUNC_LOG; + + int res = 0; + + // Enable DMP quaternion + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.ped_q_on, getTimestamp()); + if (write_sysfs_int(mpu.ped_q_on, en) < 0) { + LOGE("HAL:ERR can't write DMP ped_q_on"); + res = -1; //Indicate an err + } + + if (!en) { + LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped quat"); + //Disable Accel if no sensor needs it + if (!(mFeatureActiveMask & DMP_FEATURE_MASK) + && (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_ACCEL))) { + res = enableAccel(0); + if (res < 0) + return res; + } + if (!(mFeatureActiveMask & DMP_FEATURE_MASK) + && (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_GYRO))) { + res = enableGyro(0); + if (res < 0) + return res; + } + if (mFeatureActiveMask & INV_DMP_QUATERNION) { + res = write_sysfs_int(mpu.gyro_fifo_enable, 1); + res += write_sysfs_int(mpu.accel_fifo_enable, 1); + if (res < 0) + return res; + } + // LOGV_IF(ENG_VERBOSE, "before mLocalSensorMask=0x%lx", mLocalSensorMask); + // reset global mask for buildMpuEvent() + if (mEnabled & (1 << GameRotationVector)) { + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else if (mEnabled & (1 << Accelerometer)) { + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else if ((mEnabled & ( 1 << Gyro)) || + (mEnabled & (1 << RawGyro))) { + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } + //LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat"); + // enable accel engine + res = enableAccel(1); + if (res < 0) + return res; + + // enable gyro engine + res = enableGyro(1); + if (res < 0) + return res; + LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); + // disable accel FIFO + if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) || + !(mBatchEnabled & (1 << Accelerometer))) { + res = turnOffAccelFifo(); + if (res < 0) + return res; + mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; + } + + // disable gyro FIFO + if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) || + !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) { + res = turnOffGyroFifo(); + if (res < 0) + return res; + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + } + + return res; +} + +int MPLSensor::check6AxisQuatEnabled(void) +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0); +} + +/* This is used for batch mode only */ +/* GRV is batched but not along with ped */ +int MPLSensor::enable6AxisQuaternion(int en) +{ + VFUNC_LOG; + + if (!en) { + enable6AxisQuaternionData(0); + mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION; + if (mFeatureActiveMask == 0) { + onDmp(0); + } + LOGV_IF(ENG_VERBOSE, "HAL:6 Axis Quat disabled"); + } else { + if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) { + LOGE("HAL:ERR can't enable 6 Axis Quaternion"); + } else { + mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION; + LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled"); + } + } + return 0; +} + +int MPLSensor::enable6AxisQuaternionData(int en) +{ + VFUNC_LOG; + + int res = 0; + + // Enable DMP quaternion + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.six_axis_q_on, getTimestamp()); + if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) { + LOGE("HAL:ERR can't write DMP six_axis_q_on"); + res = -1; //Indicate an err + } + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off"); + inv_quaternion_sensor_was_turned_off(); + if (!(mFeatureActiveMask & DMP_FEATURE_MASK) + && (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_ACCEL))) { + res = enableAccel(0); + if (res < 0) + return res; + } + if (!(mFeatureActiveMask & DMP_FEATURE_MASK) + && (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_GYRO))) { + res = enableGyro(0); + if (res < 0) + return res; + } + if (mFeatureActiveMask & INV_DMP_QUATERNION) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.gyro_fifo_enable, getTimestamp()); + res = write_sysfs_int(mpu.gyro_fifo_enable, 1); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.accel_fifo_enable, getTimestamp()); + res += write_sysfs_int(mpu.accel_fifo_enable, 1); + if (res < 0) + return res; + } + LOGV_IF(ENG_VERBOSE, " k=0x%lx", mLocalSensorMask); + // reset global mask for buildMpuEvent() + if (mEnabled & (1 << GameRotationVector)) { + if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + res = write_sysfs_int(mpu.gyro_fifo_enable, 1); + res += write_sysfs_int(mpu.accel_fifo_enable, 1); + if (res < 0) + return res; + } + } else if (mEnabled & (1 << Accelerometer)) { + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else if ((mEnabled & ( 1 << Gyro)) || + (mEnabled & (1 << RawGyro))) { + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } + LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat"); + if (mEnabled & ( 1 << GameRotationVector)) { + // enable accel engine + res = enableAccel(1); + if (res < 0) + return res; + + // enable gyro engine + res = enableGyro(1); + if (res < 0) + return res; + LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask); + if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) || + (!(mBatchEnabled & (1 << Accelerometer)) || + (!(mEnabled & (1 << Accelerometer))))) { + res = turnOffAccelFifo(); + if (res < 0) + return res; + mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; + } + + if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) || + (!(mBatchEnabled & (1 << Gyro)) || + (!(mEnabled & (1 << Gyro))))) { + if (!(mBatchEnabled & (1 << RawGyro)) || + (!(mEnabled & (1 << RawGyro)))) { + res = turnOffGyroFifo(); + if (res < 0) + return res; + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + } + LOGV_IF(ENG_VERBOSE, "after: mLocalSensorMask=0x%lx", mLocalSensorMask); + } + } + + return res; +} + +/* this is for batch mode only */ +int MPLSensor::checkLPQRateSupported(void) +{ + VFUNC_LOG; +#ifndef USE_LPQ_AT_FASTEST + return ((mDelays[GameRotationVector] <= RATE_200HZ) ? 0 :1); +#else + return 1; +#endif +} + +int MPLSensor::checkLPQuaternion(void) +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); +} + +int MPLSensor::enableLPQuaternion(int en) +{ + VFUNC_LOG; + + if (!en) { + enableQuaternionData(0); + mFeatureActiveMask &= ~INV_DMP_QUATERNION; + if (mFeatureActiveMask == 0) { + onDmp(0); + } + LOGV_IF(ENG_VERBOSE, "HAL:LP Quat disabled"); + } else { + if (enableQuaternionData(1) < 0 || onDmp(1) < 0) { + LOGE("HAL:ERR can't enable LP Quaternion"); + } else { + mFeatureActiveMask |= INV_DMP_QUATERNION; + LOGV_IF(ENG_VERBOSE, "HAL:LP Quat enabled"); + } + } + return 0; +} + +int MPLSensor::enableQuaternionData(int en) +{ + VFUNC_LOG; + + int res = 0; + + // Enable DMP quaternion + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.three_axis_q_on, getTimestamp()); + if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) { + LOGE("HAL:ERR can't write DMP three_axis_q__on"); + res = -1; //Indicates an err + } + + if (!en) { + LOGV_IF(ENG_VERBOSE, "HAL:DMP quaternion data was turned off"); + inv_quaternion_sensor_was_turned_off(); + } else { + LOGV_IF(ENG_VERBOSE, "HAL:Enabling three axis quat"); + } + + return res; +} + +int MPLSensor::enableDmpPedometer(int en, int interruptMode) +{ + VFUNC_LOG; + int res = 0; + int enabled_sensors = mEnabled; + + if (isMpuNonDmp()) + return res; + + // reset master enable + res = masterEnable(0); + if (res < 0) { + return res; + } + + if (en == 1) { + //Enable DMP Pedometer Function + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.pedometer_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_on, en) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer"); + res = -1; // indicate an err + return res; + } + + if (interruptMode || (mFeatureActiveMask & INV_DMP_PEDOMETER)) { + //Enable DMP Pedometer Interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); + res = -1; // indicate an err + return res; + } + } + + if (interruptMode) { + mFeatureActiveMask |= INV_DMP_PEDOMETER; + } + else { + mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP; + mStepCountPollTime = 100000000LL; + } + + clock_gettime(CLOCK_MONOTONIC, &mt_pre); + } else { + if (interruptMode) { + mFeatureActiveMask &= ~INV_DMP_PEDOMETER; + } + else { + mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP; + mStepCountPollTime = -1; + } + + /* if neither step detector or step count is on */ + if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.pedometer_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_on, en) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer"); + res = -1; + return res; + } + } + + /* if feature is not step detector */ + if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.pedometer_int_on, getTimestamp()); + if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) { + LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); + res = -1; + return res; + } + } + } + + if ((res = setDmpFeature(en)) < 0) + return res; + + if ((res = computeAndSetDmpState()) < 0) + return res; + + if (resetDataRates() < 0) + return res; + + if(en || enabled_sensors || mFeatureActiveMask) { + res = masterEnable(1); + } + return res; +} + +int MPLSensor::masterEnable(int en) +{ + VFUNC_LOG; + + int res = 0; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.master_enable, getTimestamp()); + res = write_sysfs_int(mpu.master_enable, en); + return res; +} + +int MPLSensor::enableGyro(int en) +{ + VFUNC_LOG; + + int res = 0; + + /* need to also turn on/off the master enable */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.gyro_enable, getTimestamp()); + res = write_sysfs_int(mpu.gyro_enable, en); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.gyro_fifo_enable, getTimestamp()); + res += write_sysfs_int(mpu.gyro_fifo_enable, en); + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); + inv_gyro_was_turned_off(); + } + + return res; +} + +int MPLSensor::enableLowPowerAccel(int en) +{ + VFUNC_LOG; + + int res; + + /* need to also turn on/off the master enable */ + res = write_sysfs_int(mpu.motion_lpa_on, en); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.motion_lpa_on, getTimestamp()); + return res; +} + +int MPLSensor::enableAccel(int en) +{ + VFUNC_LOG; + + int res; + + /* need to also turn on/off the master enable */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.accel_enable, getTimestamp()); + res = write_sysfs_int(mpu.accel_enable, en); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.accel_fifo_enable, getTimestamp()); + res += write_sysfs_int(mpu.accel_fifo_enable, en); + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); + inv_accel_was_turned_off(); + } + + return res; +} + +int MPLSensor::enableCompass(int en, int rawSensorRequested) +{ + VFUNC_LOG; + + int res = 0; + /* TODO: handle ID_RM if third party compass cal is used */ + if (rawSensorRequested && mCompassSensor->providesCalibration()) { + res = mCompassSensor->enable(ID_RM, en); + } else { + res = mCompassSensor->enable(ID_M, en); + } + if (en == 0 || res != 0) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res); + inv_compass_was_turned_off(); + } + + return res; +} + +int MPLSensor::enablePressure(int en) +{ + VFUNC_LOG; + + int res = 0; + + if (mPressureSensor) + res = mPressureSensor->enable(ID_PS, en); + + return res; +} + +/* use this function for initialization */ +int MPLSensor::enableBatch(int64_t timeout) +{ + VFUNC_LOG; + + int res = 0; + + res = write_sysfs_int(mpu.batchmode_timeout, timeout); + if (timeout == 0) { + res = write_sysfs_int(mpu.six_axis_q_on, 0); + res = write_sysfs_int(mpu.ped_q_on, 0); + res = write_sysfs_int(mpu.step_detector_on, 0); + res = write_sysfs_int(mpu.step_indicator_on, 0); + } + + if (timeout == 0) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero"); + } + + return res; +} + +void MPLSensor::computeLocalSensorMask(int enabled_sensors) +{ + VFUNC_LOG; + + do { + /* Invensense Pressure on secondary bus */ + if (PS_ENABLED) { + LOGV_IF(ENG_VERBOSE, "PS ENABLED"); + mLocalSensorMask |= INV_ONE_AXIS_PRESSURE; + } else { + LOGV_IF(ENG_VERBOSE, "PS DISABLED"); + mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE; + } + + if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED + || (GRV_ENABLED && GMRV_ENABLED)) { + LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); + mLocalSensorMask |= ALL_MPL_SENSORS_NP; + break; + } + + if (GRV_ENABLED) { + if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE) || + !(mBatchEnabled & (1 << GameRotationVector))) { + LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else { + if (GY_ENABLED || RGY_ENABLED) { + LOGV_IF(ENG_VERBOSE, "G ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } else { + LOGV_IF(ENG_VERBOSE, "G DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + if (A_ENABLED) { + LOGV_IF(ENG_VERBOSE, "A ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else { + LOGV_IF(ENG_VERBOSE, "A DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; + } + } + /* takes care of MAG case */ + if (M_ENABLED || RM_ENABLED) { + LOGV_IF(1, "M ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_COMPASS; + } else { + LOGV_IF(1, "M DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; + } + break; + } + + if (GMRV_ENABLED) { + LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + mLocalSensorMask |= INV_THREE_AXIS_COMPASS; + + /* takes care of Gyro case */ + if (GY_ENABLED || RGY_ENABLED) { + LOGV_IF(1, "G ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } else { + LOGV_IF(1, "G DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + break; + } + + if(!A_ENABLED && !M_ENABLED && !RM_ENABLED && + !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED && + !PS_ENABLED) { + /* Invensense compass cal */ + LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); + mLocalSensorMask = 0; + break; + } + + if (GY_ENABLED || RGY_ENABLED) { + LOGV_IF(ENG_VERBOSE, "G ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } else { + LOGV_IF(ENG_VERBOSE, "G DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + + if (A_ENABLED) { + LOGV_IF(ENG_VERBOSE, "A ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else { + LOGV_IF(ENG_VERBOSE, "A DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; + } + + /* Invensense compass calibration */ + if (M_ENABLED || RM_ENABLED) { + LOGV_IF(ENG_VERBOSE, "M ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_COMPASS; + } else { + LOGV_IF(ENG_VERBOSE, "M DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; + } + } while (0); +} + +int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) +{ + VFUNC_LOG; + + inv_error_t res = -1; + int on = 1; + int off = 0; + int cal_stored = 0; + + // Sequence to enable or disable a sensor + // 1. reset master enable (=0) + // 2. enable or disable a sensor + // 3. set master enable (=1) + + if (isLowPowerQuatEnabled() || + changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | + (mCompassSensor->isIntegrated() << MagneticField) | + (mCompassSensor->isIntegrated() << RawMagneticField) | + (mPressureSensor->isIntegrated() << Pressure))) { + + /* reset master enable */ + res = masterEnable(0); + if(res < 0) { + return res; + } + } + + LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", + (unsigned int)sensors); + + if (changed & ((1 << Gyro) | (1 << RawGyro))) { + LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - gyro %s", + (sensors & INV_THREE_AXIS_GYRO? "enable": "disable")); + res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO)); + if(res < 0) { + return res; + } + + if (!cal_stored && (!en && (changed & (1 << Gyro)))) { + storeCalibration(); + cal_stored = 1; + } + } + + if (changed & (1 << Accelerometer)) { + LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - accel %s", + (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable")); + res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL)); + if(res < 0) { + return res; + } + + if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) { + storeCalibration(); + cal_stored = 1; + } + } + + if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) { + LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - compass %s", + (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable")); + res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField)); + if(res < 0) { + return res; + } + + if (!cal_stored && (!en && (changed & (1 << MagneticField)))) { + storeCalibration(); + cal_stored = 1; + } + } + + if (changed & (1 << Pressure)) { + LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - pressure %s", + (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable")); + res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE)); + if(res < 0) { + return res; + } + } + + if (isLowPowerQuatEnabled()) { + // Enable LP Quat + if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK) + || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) { + LOGV_IF(ENG_VERBOSE, "HAL: 9 axis or game rot enabled"); + if (!(changed & ((1 << Gyro) + | (1 << RawGyro) + | (1 << Accelerometer) + | (mCompassSensor->isIntegrated() << MagneticField) + | (mCompassSensor->isIntegrated() << RawMagneticField))) + ) { + /* reset master enable */ + res = masterEnable(0); + if(res < 0) { + return res; + } + } + if (!checkLPQuaternion()) { + enableLPQuaternion(1); + } else { + LOGV_IF(ENG_VERBOSE, "HAL:LP Quat already enabled"); + } + } else if (checkLPQuaternion()) { + enableLPQuaternion(0); + } + } + + /* apply accel/gyro bias to DMP bias */ + /* precondition: masterEnable(0), mGyroBiasAvailable=true */ + /* postcondition: bias is applied upon masterEnable(1) */ + if(!(sensors & INV_THREE_AXIS_GYRO)) { + setGyroBias(); + } + if(!(sensors & INV_THREE_AXIS_ACCEL)) { + setAccelBias(); + } + + /* to batch or not to batch */ + int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled); + /* skip setBatch if there is no need to */ + if(((int)mOldBatchEnabledMask != batchMode) || batchMode) { + setBatch(batchMode,0); + } + mOldBatchEnabledMask = batchMode; + + /* check for invn hardware sensors change */ + if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | + (mCompassSensor->isIntegrated() << MagneticField) | + (mCompassSensor->isIntegrated() << RawMagneticField) | + (mPressureSensor->isIntegrated() << Pressure))) { + LOGV_IF(ENG_VERBOSE, + "HAL DEBUG: Gyro, Accel, Compass, Pressure changes"); + if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()) + | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) { + LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled"); + LOGV_IF(ENG_VERBOSE, + "mFeatureActiveMask=0x%llx", mFeatureActiveMask); + LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled"); + // 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; + } + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=0x%llx", mFeatureActiveMask); + LOGV_IF(ENG_VERBOSE, "DMP_FEATURE_MASK=0x%x", DMP_FEATURE_MASK); + if ((mFeatureActiveMask & (long long)DMP_FEATURE_MASK) && + !((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) || + (mFeatureActiveMask & INV_DMP_PED_STANDALONE) || + (mFeatureActiveMask & INV_DMP_PED_QUATERNION) || + (mFeatureActiveMask & INV_DMP_BATCH_MODE))) { + // enable DMP + onDmp(1); + res = enableAccel(on); + if(res < 0) { + return res; + } + LOGV_IF(ENG_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); + if (((sensors | mLocalSensorMask) & INV_THREE_AXIS_ACCEL) == 0) { + res = turnOffAccelFifo(); + } + if(res < 0) { + return res; + } + } + } else { // all sensors idle + LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors"); + if (isDmpDisplayOrientationOn() + && (mDmpOrientationEnabled + || !isDmpScreenAutoRotationEnabled())) { + enableDmpOrientation(1); + } + + if (!cal_stored) { + storeCalibration(); + cal_stored = 1; + } + } + } else if ((changed & + ((!mCompassSensor->isIntegrated()) << MagneticField) || + ((!mCompassSensor->isIntegrated()) << RawMagneticField)) + && + !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated())))) + ) { + LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change"); + if (!cal_stored) { + storeCalibration(); + cal_stored = 1; + } + } /*else { + LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled"); + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { + res = masterEnable(1); + if(res < 0) + return res; + } + }*/ + + if (!batchMode && (resetDataRates() < 0)) { + LOGE("HAL:ERR can't reset output rate back to original setting"); + } + + if(mFeatureActiveMask || sensors) { + res = masterEnable(1); + if(res < 0) + return res; + } + return res; +} + +/* check if batch mode should be turned on or not */ +int MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor) +{ + VFUNC_LOG; + int batchMode = 1; + mFeatureActiveMask &= ~INV_DMP_BATCH_MODE; + + LOGV_IF(ENG_VERBOSE, + "HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d", + enableSensors, tempBatchSensor); + + // handle initialization case + if (enableSensors == 0 && tempBatchSensor == 0) + return 0; + + // check for possible continuous data mode + for(int i = 0; i <= Pressure; i++) { + // if any one of the hardware sensor is in continuous data mode + // turn off batch mode. + if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) { + LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: " + "hardware sensor on continuous mode:%d", i); + return 0; + } + if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) { + LOGV_IF(ENG_VERBOSE, + "HAL:computeBatchSensorMask: hardware sensor is batch:%d", + i); + // if hardware sensor is batched, check if virtual sensor is also batched + if ((enableSensors & (1 << GameRotationVector)) + && !(tempBatchSensor & (1 << GameRotationVector))) { + LOGV_IF(ENG_VERBOSE, + "HAL:computeBatchSensorMask: but virtual sensor is not:%d", + i); + return 0; + } + } + } + + // if virtual sensors are on but not batched, turn off batch mode. + for(int i = Orientation; i <= GeomagneticRotationVector; i++) { + if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) { + LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: " + "composite sensor on continuous mode:%d", i); + return 0; + } + } + + if ((mFeatureActiveMask & INV_DMP_PEDOMETER) && !(tempBatchSensor & (1 << StepDetector))) { + LOGV("HAL:computeBatchSensorMask: step detector on continuous mode."); + return 0; + } + + mFeatureActiveMask |= INV_DMP_BATCH_MODE; + LOGV_IF(EXTRA_VERBOSE, + "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x", + batchMode, tempBatchSensor); + return (batchMode && tempBatchSensor); +} + +/* This function is called by enable() */ +int MPLSensor::setBatch(int en, int toggleEnable) +{ + VFUNC_LOG; + + int res = 0; + int64_t wanted = 1000000000LL; + int64_t timeout = 0; + int timeoutInMs = 0; + int featureMask = computeBatchDataOutput(); + + // reset master enable + if (toggleEnable == 1) { + res = masterEnable(0); + if (res < 0) { + return res; + } + } + + if (en) { + /* take the minimum batchmode timeout */ + int64_t timeout = 100000000000LL; + int64_t ns; + for (int i = 0; i < NumSensors; i++) { + LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x", + mFeatureActiveMask, mEnabled, mBatchEnabled); + if (((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i))) || + (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) { + LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]); + ns = mBatchTimeouts[i]; + timeout = (ns < timeout) ? ns : timeout; + } + } + /* Convert ns to millisecond */ + timeoutInMs = timeout / 1000000; + } else { + timeoutInMs = 0; + } + + LOGV_IF(ENG_VERBOSE, "HAL: batch timeout set to %dms", timeoutInMs); + + /* step detector is enabled and */ + /* batch mode is standalone */ + if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) && + (featureMask & INV_DMP_PED_STANDALONE)) { + LOGV_IF(ENG_VERBOSE, "ID_P only = 0x%x", mBatchEnabled); + enablePedStandalone(1); + } else { + enablePedStandalone(0); + } + + /* step detector and GRV are enabled and */ + /* batch mode is ped q */ + if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) && + (mEnabled & (1 << GameRotationVector)) && + (featureMask & INV_DMP_PED_QUATERNION)) { + LOGV_IF(ENG_VERBOSE, "ID_P and GRV or ALL = 0x%x", mBatchEnabled); + LOGV_IF(ENG_VERBOSE, "ID_P is enabled for batching, " + "PED quat will be automatically enabled"); + enableLPQuaternion(0); + enablePedQuaternion(1); + } else if (!(featureMask & INV_DMP_PED_STANDALONE)){ + enablePedQuaternion(0); + } else { + enablePedQuaternion(0); + } + + /* step detector and hardware sensors enabled */ + if (en && (featureMask & INV_DMP_PED_INDICATOR) && + ((mEnabled) || + (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) { + enablePedIndicator(1); + } else { + enablePedIndicator(0); + } + + /* GRV is enabled and */ + /* batch mode is 6axis q */ + if (en && (mEnabled & (1 << GameRotationVector)) && + (featureMask & INV_DMP_6AXIS_QUATERNION)) { + LOGV_IF(ENG_VERBOSE, "GRV = 0x%x", mBatchEnabled); + enableLPQuaternion(0); + enable6AxisQuaternion(1); + setInitial6QuatValue(); + } else if (!(featureMask & INV_DMP_PED_QUATERNION)){ + LOGV_IF(ENG_VERBOSE, "Toggle back to normal 6 axis"); + if (mEnabled & (1 << GameRotationVector)) { + enableLPQuaternion(checkLPQRateSupported()); + } + enable6AxisQuaternion(0); + } else { + enable6AxisQuaternion(0); + } + + /* write required timeout to sysfs */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + timeoutInMs, mpu.batchmode_timeout, getTimestamp()); + if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) { + LOGE("HAL:ERR can't write batchmode_timeout"); + } + + if (en) { + // enable DMP + res = onDmp(1); + if (res < 0) { + return res; + } + + // set batch rates + if (setBatchDataRates() < 0) { + LOGE("HAL:ERR can't set batch data rates"); + } + + // default fifo rate to 200Hz + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 200, mpu.gyro_fifo_rate, getTimestamp()); + if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) { + res = -1; + LOGE("HAL:ERR can't set rate to 200Hz"); + return res; + } + } else { + if (mFeatureActiveMask == 0) { + // disable DMP + res = onDmp(0); + if (res < 0) { + return res; + } + /* reset sensor rate */ + if (resetDataRates() < 0) { + LOGE("HAL:ERR can't reset output rate back to original setting"); + } + } + } + + // set sensor data interrupt + uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE)); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + !dataInterrupt, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + + if (toggleEnable == 1) { + if (mFeatureActiveMask || mEnabled) + masterEnable(1); + } + return res; +} + +/* Store calibration file */ +void MPLSensor::storeCalibration(void) +{ + VFUNC_LOG; + + if(mHaveGoodMpuCal == true + || mAccelAccuracy >= 2 + || mCompassAccuracy >= 3) { + int res = inv_store_calibration(); + if (res) { + LOGE("HAL:Cannot store calibration on file"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated"); + } + } +} + +/* these handlers transform mpl data into one of the Android sensor types */ +int MPLSensor::gyroHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, + &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::rawGyroHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib, + &s->gyro.status, &s->timestamp); + if(update) { + memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias)); + LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d", + s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1], + s->uncalibrated_gyro.bias[2], s->timestamp, update); + } + s->gyro.status = SENSOR_STATUS_UNRELIABLE; + LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d", + s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1], + s->uncalibrated_gyro.uncalib[2], s->timestamp, update); + return update; +} + +int MPLSensor::accelHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + 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->timestamp, update); + mAccelAccuracy = s->acceleration.status; + return update; +} + +int MPLSensor::compassHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_magnetic_field( + 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; +} + +int MPLSensor::rawCompassHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + //TODO: need to handle uncalib data and bias for 3rd party compass + if(mCompassSensor->providesCalibration()) { + update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp); + } + else { + update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib, + &s->magnetic.status, &s->timestamp); + } + if(update) { + memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias)); + LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d", + s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1], + s->uncalibrated_magnetic.bias[2], s->timestamp, update); + } + s->magnetic.status = SENSOR_STATUS_UNRELIABLE; + LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d", + s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1], + s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update); + return update; +} + +/* + Rotation Vector handler. + NOTE: rotation vector does not have an accuracy or status +*/ +int MPLSensor::rvHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_rotation_vector(s->data, &status, + &s->timestamp); + update |= isCompassDisabled(); + LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f- %+lld - %d", + s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, + update); + + return update; +} + +/* + Game Rotation Vector handler. + NOTE: rotation vector does not have an accuracy or status +*/ +int MPLSensor::grvHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status, + &s->timestamp); + + LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f - %+lld - %d", + s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, + update); + return update; +} + +int MPLSensor::laHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_linear_acceleration( + s->gyro.v, &s->gyro.status, &s->timestamp); + update |= isCompassDisabled(); + LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::gravHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, + &s->timestamp); + update |= isCompassDisabled(); + LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::orienHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_orientation( + s->orientation.v, &s->orientation.status, &s->timestamp); + update |= isCompassDisabled(); + LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d", + s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], + s->timestamp, update); + return update; +} + +int MPLSensor::smHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update = 1; + + /* When event is triggered, set data to 1 */ + s->data[0] = 1.f; + s->data[1] = 0.f; + s->data[2] = 0.f; + + /* Capture timestamp in HAL */ + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; + + LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d", + s->data[0], s->timestamp, update); + return update; +} + +int MPLSensor::gmHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update = 0; + update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status, + &s->timestamp); + + LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f- %+lld - %d", + s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, update); + return update < 1 ? 0 :1; + +} + +int MPLSensor::psHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update = 0; + + s->pressure = mCachedPressureData / 100.f; //hpa (millibar) + s->data[1] = 0; + s->data[2] = 0; + s->timestamp = mPressureTimestamp; + s->magnetic.status = 0; + update = mPressureUpdate; + mPressureUpdate = 0; + + LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d", + s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update); + return update < 1 ? 0 :1; + +} + +int MPLSensor::sdHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update = 1; + + /* When event is triggered, set data to 1 */ + s->data[0] = 1; + s->data[1] = 0.f; + s->data[2] = 0.f; + + /* get current timestamp */ + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts) ; + s->timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec; + + LOGV_IF(HANDLER_DATA, "HAL:sd data: %f - %lld - %d", + s->data[0], s->timestamp, update); + return update; +} + +int MPLSensor::scHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update = 1; + + /* Set step count */ +#if defined ANDROID_KITKAT + s->u64.step_counter = mLastStepCount; + LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d", + s->u64.step_counter, s->timestamp, update); +#else + s->step_counter = mLastStepCount; + LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d", + s->step_counter, s->timestamp, update); +#endif + return update; +} + +int MPLSensor::metaHandler(sensors_event_t* s, int flags) +{ + VHANDLER_LOG; + int update = 0; + +#if defined ANDROID_KITKAT + /* initalize SENSOR_TYPE_META_DATA */ + s->version = 0; + s->sensor = 0; + s->reserved0 = 0; + s->timestamp = 0LL; + + switch(flags) { + case META_DATA_FLUSH_COMPLETE: + update = mFlushBatchSet; + s->type = SENSOR_TYPE_META_DATA; + s->meta_data.what = flags; + s->meta_data.sensor = mFlushSensorEnabled; + mFlushBatchSet = 0; + mFlushSensorEnabled = -1; + LOGV_IF(HANDLER_DATA, + "HAL:flush complete data: type=%d what=%d, " + "sensor=%d - %lld - %d", + s->type, s->meta_data.what, s->meta_data.sensor, + s->timestamp, update); + break; + + default: + LOGW("HAL: Meta flags not supported"); + break; + } +#endif + return update; +} + +int MPLSensor::enable(int32_t handle, int en) +{ + VFUNC_LOG; + + android::String8 sname; + int what = -1, err = 0; + int batchMode = 0; + + if (uint32_t(handle) >= NumSensors) + return -EINVAL; + + if (!en) + mBatchEnabled &= ~(1 << handle); + + LOGV_IF(ENG_VERBOSE, "HAL:handle = %d", handle); + + switch (handle) { + case ID_SC: + what = StepCounter; + sname = "Step Counter"; + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", + sname.string(), handle, + (mDmpStepCountEnabled? "en": "dis"), + (en? "en" : "dis")); + enableDmpPedometer(en, 0); + mDmpStepCountEnabled = !!en; + return 0; + case ID_P: + sname = "StepDetector"; + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", + sname.string(), handle, + (mDmpPedometerEnabled? "en": "dis"), + (en? "en" : "dis")); + enableDmpPedometer(en, 1); + mDmpPedometerEnabled = !!en; + batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled); + /* skip setBatch if there is no need to */ + if(((int)mOldBatchEnabledMask != batchMode) || batchMode) { + setBatch(batchMode,1); + } + mOldBatchEnabledMask = batchMode; + return 0; + case ID_SM: + sname = "Significant Motion"; + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", + sname.string(), handle, + (mDmpSignificantMotionEnabled? "en": "dis"), + (en? "en" : "dis")); + enableDmpSignificantMotion(en); + mDmpSignificantMotionEnabled = !!en; + return 0; + 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"; + break; + case ID_M: + what = MagneticField; + sname = "MagneticField"; + break; + case ID_RM: + what = RawMagneticField; + sname = "MagneticField Uncalibrated"; + break; + case ID_O: + what = Orientation; + sname = "Orientation"; + break; + case ID_GY: + what = Gyro; + sname = "Gyro"; + break; + case ID_RG: + what = RawGyro; + sname = "Gyro Uncalibrated"; + break; + case ID_GR: + what = Gravity; + sname = "Gravity"; + break; + case ID_RV: + what = RotationVector; + sname = "RotationVector"; + break; + case ID_GRV: + what = GameRotationVector; + sname = "GameRotationVector"; + break; + case ID_LA: + what = LinearAccel; + sname = "LinearAccel"; + break; + case ID_GMRV: + what = GeomagneticRotationVector; + sname = "GeomagneticRotationVector"; + break; + case ID_PS: + what = Pressure; + sname = "Pressure"; + break; + default: + what = handle; + sname = "Others"; + break; + } + + int newState = en ? 1 : 0; + unsigned long sen_mask; + + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", + sname.string(), handle, + ((mEnabled & (1 << what)) ? "en" : "dis"), + ((uint32_t(newState) << what) ? "en" : "dis")); + LOGV_IF(ENG_VERBOSE, + "HAL:%s sensor state change what=%d", sname.string(), what); + + // pthread_mutex_lock(&mMplMutex); + // pthread_mutex_lock(&mHALMutex); + + if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { + uint32_t sensor_type; + short flags = newState; + uint32_t lastEnabled = mEnabled, changed = 0; + + mEnabled &= ~(1 << what); + mEnabled |= (uint32_t(flags) << what); + + LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags); + computeLocalSensorMask(mEnabled); + LOGV_IF(ENG_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled); + LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled); + sen_mask = mLocalSensorMask & mMasterSensorMask; + mSensorMask = sen_mask; + LOGV_IF(ENG_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask); + + switch (what) { + case Gyro: + case RawGyro: + case Accelerometer: + if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) && + !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) && + ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { + changed |= (1 << what); + } + if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) { + changed |= (1 << what); + } + break; + case MagneticField: + case RawMagneticField: + if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) && + ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { + changed |= (1 << what); + } + break; + case Pressure: + if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) { + changed |= (1 << what); + } + break; + case GameRotationVector: + if (!en) + storeCalibration(); + if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK)) + || + (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK)) + || + (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) + || + (!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) { + for (int i = Gyro; i <= RawMagneticField; i++) { + if (!(mEnabled & (1 << i))) { + changed |= (1 << i); + } + } + } + break; + + case Orientation: + case RotationVector: + case LinearAccel: + case Gravity: + if (!en) + storeCalibration(); + if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK)) + || + (!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) { + for (int i = Gyro; i <= RawMagneticField; i++) { + if (!(mEnabled & (1 << i))) { + changed |= (1 << i); + } + } + } + break; + case GeomagneticRotationVector: + if (!en) + storeCalibration(); + if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK)) + || + (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK)) + || + (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) + || + (!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) { + for (int i = Accelerometer; i <= RawMagneticField; i++) { + if (!(mEnabled & (1<<i))) { + changed |= (1 << i); + } + } + } + break; + } + LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed); + enableSensors(sen_mask, flags, changed); + } + + // pthread_mutex_unlock(&mMplMutex); + // pthread_mutex_unlock(&mHALMutex); + +#ifdef INV_PLAYBACK_DBG + /* apparently the logging needs to go through this sequence + to properly flush the log file */ + inv_turn_off_data_logging(); + if (fclose(logfile) < 0) { + LOGE("cannot close debug log file"); + } + logfile = fopen("/data/playback.bin", "ab"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif + + return err; +} + +void MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname) +{ + VFUNC_LOG; + + what = -1; + + switch (handle) { + case ID_P: + what = StepDetector; + sname = "StepDetector"; + break; + case ID_SC: + what = StepCounter; + sname = "StepCounter"; + break; + case ID_SM: + what = SignificantMotion; + sname = "SignificantMotion"; + break; + case ID_SO: + what = handle; + sname = "ScreenOrienation"; + break; + case ID_A: + what = Accelerometer; + sname = "Accelerometer"; + break; + case ID_M: + what = MagneticField; + sname = "MagneticField"; + break; + case ID_RM: + what = RawMagneticField; + sname = "MagneticField Uncalibrated"; + break; + case ID_O: + what = Orientation; + sname = "Orientation"; + break; + case ID_GY: + what = Gyro; + sname = "Gyro"; + break; + case ID_RG: + what = RawGyro; + sname = "Gyro Uncalibrated"; + break; + case ID_GR: + what = Gravity; + sname = "Gravity"; + break; + case ID_RV: + what = RotationVector; + sname = "RotationVector"; + break; + case ID_GRV: + what = GameRotationVector; + sname = "GameRotationVector"; + break; + case ID_LA: + what = LinearAccel; + sname = "LinearAccel"; + break; + case ID_PS: + what = Pressure; + sname = "Pressure"; + break; + default: // this takes care of all the gestures + what = handle; + sname = "Others"; + break; + } + + LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string()); + return; +} + +int MPLSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + + android::String8 sname; + int what = -1; + +#if 0 + // skip the 1st call for enalbing sensors called by ICS/JB sensor service + static int counter_delay = 0; + if (!(mEnabled & (1 << what))) { + counter_delay = 0; + } else { + if (++counter_delay == 1) { + return 0; + } + else { + counter_delay = 0; + } + } +#endif + + getHandle(handle, what, sname); + if (uint32_t(what) >= NumSensors) + return -EINVAL; + + if (ns < 0) + return -EINVAL; + + LOGV_IF(PROCESS_VERBOSE, + "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns); + + // limit all rates to reasonable ones */ + if (ns < 5000000LL) { + ns = 5000000LL; + } + + /* store request rate to mDelays arrary for each sensor */ + int64_t previousDelay = mDelays[what]; + mDelays[what] = ns; + LOGV_IF(ENG_VERBOSE, "storing mDelays[%d] = %lld, previousDelay = %lld", what, ns, previousDelay); + + switch (what) { + case StepCounter: + /* set limits of delivery rate of events */ + mStepCountPollTime = ns; + LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns); + break; + case StepDetector: + case SignificantMotion: +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + case ID_SO: +#endif + LOGV_IF(ENG_VERBOSE, "Step Detect, SMD, SO rate=%lld ns", ns); + break; + case Gyro: + case RawGyro: + case Accelerometer: + // need to update delay since they are different + // resetDataRates was called earlier + // LOGV("what=%d mEnabled=%d mDelays[%d]=%lld previousDelay=%lld", + // what, mEnabled, what, mDelays[what], previousDelay); + if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) { + LOGV_IF(ENG_VERBOSE, + "HAL:need to update delay due to resetDataRates"); + break; + } + for (int i = Gyro; + i <= Accelerometer + mCompassSensor->isIntegrated(); + i++) { + if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { + LOGV_IF(ENG_VERBOSE, + "HAL:ignore delay set due to sensor %d", i); + return 0; + } + } + break; + + case MagneticField: + case RawMagneticField: + // need to update delay since they are different + // resetDataRates was called earlier + if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) { + LOGV_IF(ENG_VERBOSE, + "HAL:need to update delay due to resetDataRates"); + break; + } + if (mCompassSensor->isIntegrated() && + (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || + ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) || + ((mEnabled & (1 << Accelerometer)) && + ns > mDelays[Accelerometer])) && + !checkBatchEnabled()) { + /* if request is slower rate, ignore request */ + LOGV_IF(ENG_VERBOSE, + "HAL:ignore delay set due to gyro/accel"); + return 0; + } + break; + + case Orientation: + case RotationVector: + case GameRotationVector: + case GeomagneticRotationVector: + case LinearAccel: + case Gravity: + if (isLowPowerQuatEnabled()) { + LOGV_IF(ENG_VERBOSE, + "HAL:need to update delay due to LPQ"); + break; + } + + for (int i = 0; i < NumSensors; i++) { + if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { + LOGV_IF(ENG_VERBOSE, + "HAL:ignore delay set due to sensor %d", i); + return 0; + } + } + break; + } + + // pthread_mutex_lock(&mHALMutex); + int res = update_delay(); + // pthread_mutex_unlock(&mHALMutex); + return res; +} + +int MPLSensor::update_delay(void) +{ + VFUNC_LOG; + + int res = 0; + int64_t got; + + if (mEnabled) { + int64_t wanted = 1000000000LL; + int64_t wanted_3rd_party_sensor = 1000000000LL; + + // Sequence to change sensor's FIFO rate + // 1. reset master enable + // 2. Update delay + // 3. set master enable + + // reset master enable + masterEnable(0); + + int64_t gyroRate; + int64_t accelRate; + int64_t compassRate; + int64_t pressureRate; + + int rateInus; + int mplGyroRate; + int mplAccelRate; + int mplCompassRate; + int tempRate = wanted; + + /* search the minimum delay requested across all enabled sensors */ + for (int i = 0; i < NumSensors; i++) { + if (mEnabled & (1 << i)) { + int64_t ns = mDelays[i]; + wanted = wanted < ns ? wanted : ns; + } + } + + /* initialize rate for each sensor */ + gyroRate = wanted; + accelRate = wanted; + compassRate = wanted; + pressureRate = wanted; + // same delay for 3rd party Accel or Compass + wanted_3rd_party_sensor = wanted; + + int enabled_sensors = mEnabled; + int tempFd = -1; + + if(mFeatureActiveMask & INV_DMP_BATCH_MODE) { + // set batch rates + LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask); + LOGV("HAL: batch mode is set, set batch data rates"); + if (setBatchDataRates() < 0) { + LOGE("HAL:ERR can't set batch data rates"); + } + } else { + /* set master sampling frequency */ + int64_t tempWanted = wanted; + getDmpRate(&tempWanted); + /* driver only looks at sampling frequency if DMP is off */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp()); + tempFd = open(mpu.gyro_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted); + LOGE_IF(res < 0, "HAL:sampling frequency update delay error"); + + if (LA_ENABLED || GR_ENABLED || RV_ENABLED + || GRV_ENABLED || O_ENABLED || GMRV_ENABLED) { + rateInus = (int)wanted / 1000LL; + + /* set rate in MPL */ + /* compass can only do 100Hz max */ + inv_set_gyro_sample_rate(rateInus); + inv_set_accel_sample_rate(rateInus); + inv_set_compass_sample_rate(rateInus); + inv_set_linear_acceleration_sample_rate(rateInus); + inv_set_orientation_sample_rate(rateInus); + inv_set_rotation_vector_sample_rate(rateInus); + inv_set_gravity_sample_rate(rateInus); + inv_set_orientation_geomagnetic_sample_rate(rateInus); + inv_set_rotation_vector_6_axis_sample_rate(rateInus); + inv_set_geomagnetic_rotation_vector_sample_rate(rateInus); + + LOGV_IF(ENG_VERBOSE, + "HAL:MPL virtual sensor sample rate: (mpl)=%d us (mpu)=%.2f Hz", + rateInus, 1000000000.f / wanted); + LOGV_IF(ENG_VERBOSE, + "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", + rateInus, 1000000000.f / gyroRate); + LOGV_IF(ENG_VERBOSE, + "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", + rateInus, 1000000000.f / accelRate); + LOGV_IF(ENG_VERBOSE, + "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", + rateInus, 1000000000.f / compassRate); + + LOGV_IF(ENG_VERBOSE, + "mFeatureActiveMask=%016llx", mFeatureActiveMask); + if(mFeatureActiveMask & DMP_FEATURE_MASK) { + bool setDMPrate= 0; + gyroRate = wanted; + accelRate = wanted; + compassRate = wanted; + // same delay for 3rd party Accel or Compass + wanted_3rd_party_sensor = wanted; + rateInus = (int)wanted / 1000LL; + // Set LP Quaternion sample rate if enabled + if (checkLPQuaternion()) { + if (wanted <= RATE_200HZ) { +#ifndef USE_LPQ_AT_FASTEST + enableLPQuaternion(0); +#endif + } else { + inv_set_quat_sample_rate(rateInus); + LOGV_IF(ENG_VERBOSE, "HAL:MPL quat sample rate: " + "(mpl)=%d us (mpu)=%.2f Hz", + rateInus, 1000000000.f / wanted); + setDMPrate= 1; + } + } + } + + LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); + //nsToHz + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / gyroRate, mpu.gyro_rate, + getTimestamp()); + tempFd = open(mpu.gyro_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate); + if(res < 0) { + LOGE("HAL:GYRO update delay error"); + } + + if(USE_THIRD_PARTY_ACCEL == 1) { + // 3rd party accelerometer - if applicable + // nsToHz (BMA250) + LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", + wanted_3rd_party_sensor / 1000000L, mpu.accel_rate, + getTimestamp()); + tempFd = open(mpu.accel_rate, O_RDWR); + res = write_attribute_sensor(tempFd, + wanted_3rd_party_sensor / 1000000L); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } else { + // mpu accel + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / accelRate, mpu.accel_rate, + getTimestamp()); + tempFd = open(mpu.accel_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / accelRate); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } + + if (!mCompassSensor->isIntegrated()) { + // stand-alone compass - if applicable + LOGV_IF(ENG_VERBOSE, + "HAL:Ext compass delay %lld", wanted_3rd_party_sensor); + LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz", + 1000000000.f / wanted_3rd_party_sensor); + if (wanted_3rd_party_sensor < + mCompassSensor->getMinDelay() * 1000LL) { + wanted_3rd_party_sensor = + mCompassSensor->getMinDelay() * 1000LL; + } + LOGV_IF(ENG_VERBOSE, + "HAL:Ext compass delay %lld", wanted_3rd_party_sensor); + LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz", + 1000000000.f / wanted_3rd_party_sensor); + mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor); + got = mCompassSensor->getDelay(ID_M); + inv_set_compass_sample_rate(got / 1000); + } else { + // compass on secondary bus + if (compassRate < mCompassSensor->getMinDelay() * 1000LL) { + compassRate = mCompassSensor->getMinDelay() * 1000LL; + } + mCompassSensor->setDelay(ID_M, compassRate); + } + } else { + + if (GY_ENABLED || RGY_ENABLED) { + wanted = (mDelays[Gyro] <= mDelays[RawGyro]? + (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): + (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + inv_set_gyro_sample_rate((int)wanted/1000LL); + LOGV_IF(ENG_VERBOSE, + "HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL)); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / wanted, mpu.gyro_rate, getTimestamp()); + tempFd = open(mpu.gyro_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / wanted); + LOGE_IF(res < 0, "HAL:GYRO update delay error"); + } + + if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */ + if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { + wanted = mDelays[Gyro]; + } else if (RGY_ENABLED && mDelays[RawGyro] + < mDelays[Accelerometer]) { + wanted = mDelays[RawGyro]; + } else { + wanted = mDelays[Accelerometer]; + } + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + inv_set_accel_sample_rate((int)wanted/1000LL); + LOGV_IF(ENG_VERBOSE, "HAL:MPL accel sample rate: (mpl)=%d us", + int(wanted/1000LL)); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / wanted, mpu.accel_rate, + getTimestamp()); + tempFd = open(mpu.accel_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); + } + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } + + /* Invensense compass calibration */ + if (M_ENABLED || RM_ENABLED) { + int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]? + (mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]): + (mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField])); + if (!mCompassSensor->isIntegrated()) { + wanted = compassWanted; + } else { + if (GY_ENABLED + && (mDelays[Gyro] < compassWanted)) { + wanted = mDelays[Gyro]; + } else if (RGY_ENABLED + && mDelays[RawGyro] < compassWanted) { + wanted = mDelays[RawGyro]; + } else if (A_ENABLED && mDelays[Accelerometer] + < compassWanted) { + wanted = mDelays[Accelerometer]; + } else { + wanted = compassWanted; + } + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + } + + mCompassSensor->setDelay(ID_M, wanted); + got = mCompassSensor->getDelay(ID_M); + inv_set_compass_sample_rate(got / 1000); + LOGV_IF(ENG_VERBOSE, "HAL:MPL compass sample rate: (mpl)=%d us", + int(got/1000LL)); + } + + if (PS_ENABLED) { + int64_t pressureRate = mDelays[Pressure]; + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + mPressureSensor->setDelay(ID_PS, pressureRate); + LOGE_IF(res < 0, "HAL:PRESSURE update delay error"); + } + } + + } //end of non batch mode + + unsigned long sensors = mLocalSensorMask & mMasterSensorMask; + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated() + | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) { + LOGV_IF(ENG_VERBOSE, "sensors=%lu", sensors); + res = masterEnable(1); + if(res < 0) + return res; + } else { // all sensors idle -> reduce power, unless DMP is needed + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + if(mFeatureActiveMask & DMP_FEATURE_MASK) { + res = masterEnable(1); + if(res < 0) + return res; + } + } + } + + return res; +} + +/* For Third Party Accel Input Subsystem Drivers only */ +int MPLSensor::readAccelEvents(sensors_event_t* data, int count) +{ + VHANDLER_LOG; + + if (count < 1) + return -EINVAL; + + ssize_t n = mAccelInputReader.fill(accel_fd); + if (n < 0) { + LOGE("HAL:missed accel events, exit"); + return n; + } + + int numEventReceived = 0; + input_event const* event; + int nb, done = 0; + + while (done == 0 && count && mAccelInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_ABS) { + if (event->code == EVENT_TYPE_ACCEL_X) { + mPendingMask |= 1 << Accelerometer; + mCachedAccelData[0] = event->value; + } else if (event->code == EVENT_TYPE_ACCEL_Y) { + mPendingMask |= 1 << Accelerometer; + mCachedAccelData[1] = event->value; + } else if (event->code == EVENT_TYPE_ACCEL_Z) { + mPendingMask |= 1 << Accelerometer; + mCachedAccelData[2] =event-> value; + } + } else if (type == EV_SYN) { + done = 1; + if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { + inv_build_accel(mCachedAccelData, 0, getTimestamp()); + } + } else { + LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)", + type, event->code); + } + mAccelInputReader.next(); + } + + return numEventReceived; +} + +/** + * Should be called after reading at least one of gyro + * compass or accel data. (Also okay for handling all of them). + * @returns 0, if successful, error number if not. + */ +int MPLSensor::readEvents(sensors_event_t* data, int count) +{ + VHANDLER_LOG; + + inv_execute_on_data(); + + int numEventReceived = 0; + + long msg; + msg = inv_get_message_level_0(1); + if (msg) { + if (msg & INV_MSG_MOTION_EVENT) { + LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n"); + } + if (msg & INV_MSG_NO_MOTION_EVENT) { + LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n"); + /* after the first no motion, the gyro should be + calibrated well */ + mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH; + /* if gyros are on and we got a no motion, set a flag + indicating that the cal file can be written. */ + mHaveGoodMpuCal = true; + } + if(msg & INV_MSG_NEW_AB_EVENT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Accel Bias *****\n"); + getAccelBias(); + mAccelAccuracy = inv_get_accel_accuracy(); + } + if(msg & INV_MSG_NEW_GB_EVENT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Gyro Bias *****\n"); + getGyroBias(); + setGyroBias(); + } + if(msg & INV_MSG_NEW_FGB_EVENT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Gyro Bias *****\n"); + getFactoryGyroBias(); + } + if(msg & INV_MSG_NEW_FAB_EVENT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Accel Bias *****\n"); + getFactoryAccelBias(); + } + if(msg & INV_MSG_NEW_CB_EVENT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Compass Bias *****\n"); + getCompassBias(); + mCompassAccuracy = inv_get_mag_accuracy(); + } + } + + // handle partial packet read and end marker + // skip readEvents from hal_outputs + if (mSkipReadEvents) { + if(mDataMarkerDetected || mEmptyDataMarkerDetected) { + if (!mEmptyDataMarkerDetected) { + // turn off sensors in data_builder + resetMplStates(); + } + mEmptyDataMarkerDetected = 0; + mDataMarkerDetected = 0; + + // handle flush complete event + if(mFlushBatchSet && mFlushSensorEnabled != -1) { + sensors_event_t temp; +#if defined ANDROID_KITKAT + int sendEvent = metaHandler(&temp, META_DATA_FLUSH_COMPLETE); +#else + int sendEvent = metaHandler(&temp, 0); +#endif + + if(sendEvent == 1 && count > 0) { + *data++ = temp; + count--; + numEventReceived++; + } + } + } + return numEventReceived; + } + + for (int i = 0; i < NumSensors; i++) { + int update = 0; + + // handle step detector when ped_q is enabled + if(mPedUpdate) { + if (i == StepDetector) { + update = readDmpPedometerEvents(data, count, ID_P, 1); + mPedUpdate = 0; + if(update == 1 && count > 0) { + data->timestamp = mStepSensorTimestamp; + count--; + numEventReceived++; + continue; + } + } else { + if (mPedUpdate == DATA_FORMAT_STEP) { + continue; + } + } + } + + // load up virtual sensors + if (mEnabled & (1 << i)) { + update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i); + mPendingMask |= (1 << i); + + if (update && (count > 0)) { + *data++ = mPendingEvents[i]; + count--; + numEventReceived++; + } + } + } + + return numEventReceived; +} + +// collect data for MPL (but NOT sensor service currently), from driver layer +void MPLSensor::buildMpuEvent(void) +{ + VHANDLER_LOG; + + mSkipReadEvents = 0; + int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0; + int lp_quaternion_on = 0, sixAxis_quaternion_on = 0, + ped_quaternion_on = 0, ped_standalone_on = 0; + size_t nbyte; + unsigned short data_format = 0; + int i, nb, mask = 0, + sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) + + (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) + && mCompassSensor->isIntegrated())? 1 : 0) + + ((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0); + //LOGV("mLocalSensorMask=0x%lx", mLocalSensorMask); + char *rdata = mIIOBuffer; + ssize_t rsize = 0; + ssize_t readCounter = 0; + char *rdataP = NULL; + bool doneFlag = 0; + + if (isLowPowerQuatEnabled()) { + lp_quaternion_on = checkLPQuaternion(); + } + sixAxis_quaternion_on = check6AxisQuatEnabled(); + ped_quaternion_on = checkPedQuatEnabled(); + ped_standalone_on = checkPedStandaloneEnabled(); + + nbyte = MAX_READ_SIZE - mLeftOverBufferSize; + + /* check previous copied buffer */ + /* append with just read data */ + if (mLeftOverBufferSize > 0) { + LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize); + memset(rdata, 0, sizeof(rdata)); + memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize); + LOGV_IF(0, + "HAL:input retrieve old buffer data=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, " + "%d, %d,%d, %d, %d, %d\n", + rdata[0], rdata[1], rdata[2], rdata[3], + rdata[4], rdata[5], rdata[6], rdata[7], + rdata[8], rdata[9], rdata[10], rdata[11], + rdata[12], rdata[13], rdata[14], rdata[15]); + } + rdataP = rdata + mLeftOverBufferSize; + + /* read expected number of bytes */ + rsize = read(iio_fd, rdataP, nbyte); + if(rsize < 0) { + /* IIO buffer might have old data. + Need to flush it if no sensor is on, to avoid infinite + read loop.*/ + LOGE("HAL:input data file descriptor not available - (%s)", + strerror(errno)); + if (sensors == 0) { + rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE); + if(rsize > 0) { + LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize); +#ifdef TESTING + LOGV_IF(INPUT_DATA, + "HAL:input rdata:r=%d, n=%d," + "%d, %d, %d, %d, %d, %d, %d, %d", + (int)rsize, nbyte, + rdataP[0], rdataP[1], rdataP[2], rdataP[3], + rdataP[4], rdataP[5], rdataP[6], rdataP[7]); +#endif + mLeftOverBufferSize = 0; + } + } + return; + } + +#ifdef TESTING +LOGV_IF(INPUT_DATA, + "HAL:input just read rdataP:r=%d, n=%d," + "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d," + "%d, %d, %d, %d,%d, %d, %d, %d\n", + (int)rsize, nbyte, + rdataP[0], rdataP[1], rdataP[2], rdataP[3], + rdataP[4], rdataP[5], rdataP[6], rdataP[7], + rdataP[8], rdataP[9], rdataP[10], rdataP[11], + rdataP[12], rdataP[13], rdataP[14], rdataP[15], + rdataP[16], rdataP[17], rdataP[18], rdataP[19], + rdataP[20], rdataP[21], rdataP[22], rdataP[23]); + + LOGV_IF(INPUT_DATA, + "HAL:input rdata:r=%d, n=%d," + "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d," + "%d, %d, %d, %d,%d, %d, %d, %d\n", + (int)rsize, nbyte, + rdata[0], rdata[1], rdata[2], rdata[3], + rdata[4], rdata[5], rdata[6], rdata[7], + rdata[8], rdata[9], rdata[10], rdata[11], + rdata[12], rdata[13], rdata[14], rdata[15], + rdata[16], rdata[17], rdata[18], rdata[19], + rdata[20], rdata[21], rdata[22], rdata[23]); +#endif + + if(rsize + mLeftOverBufferSize < MAX_READ_SIZE) { + /* store packet then return */ + mLeftOverBufferSize += rsize; + memcpy(mLeftOverBuffer, rdata, mLeftOverBufferSize); + +#ifdef TESTING + LOGV_IF(1, "HAL:input data has batched partial packet"); + LOGV_IF(1, "HAL:input data batched mLeftOverBufferSize=%d", mLeftOverBufferSize); + LOGV_IF(1, + "HAL:input catch up batched retrieve buffer=:%d, %d, %d, %d, %d, %d, %d, %d," + "%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n", + mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3], + mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7], + mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11], + mLeftOverBuffer[12], mLeftOverBuffer[13], mLeftOverBuffer[14], mLeftOverBuffer[15]); +#endif + mSkipReadEvents = 1; + return; + } + + /* reset data and count pointer */ + rdataP = rdata; + readCounter = rsize + mLeftOverBufferSize; + LOGV_IF(0, "HAL:input readCounter set=%d", (int)readCounter); + + LOGV_IF(INPUT_DATA && ENG_VERBOSE, + "HAL:input b=%d rdata= %d nbyte= %d rsize= %d readCounter= %d", + checkBatchEnabled(), *((short *) rdata), nbyte, (int)rsize, (int)readCounter); + LOGV_IF(INPUT_DATA && ENG_VERBOSE, + "HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, " + "ped_q_on= %d, ped_standalone_on= %d", + sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on, + ped_standalone_on); + + while (readCounter > 0) { + // since copied buffer is already accounted for, reset left over size + mLeftOverBufferSize = 0; + // clear data format mask for parsing the next set of data + mask = 0; + data_format = *((short *)(rdata)); + LOGV_IF(INPUT_DATA && ENG_VERBOSE, + "HAL:input data_format=%x", data_format); + + if(checkValidHeader(data_format) == 0) { + LOGE("HAL:input invalid data_format 0x%02X", data_format); + return; + } + + if (data_format & DATA_FORMAT_STEP) { + if (data_format == DATA_FORMAT_STEP) { + rdata += BYTES_PER_SENSOR; + latestTimestamp = *((long long*) (rdata)); + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STEP DETECTED:0x%x - ts: %lld", data_format, latestTimestamp); + // readCounter is decrement by 24 because DATA_FORMAT_STEP only applies in batch mode + readCounter -= BYTES_PER_SENSOR_PACKET; + } else { + LOGV_IF(0, "STEP DETECTED:0x%x", data_format); + } + mPedUpdate |= data_format; + mask |= DATA_FORMAT_STEP; + // cancels step bit + data_format &= (~DATA_FORMAT_STEP); + } + + if (data_format & DATA_FORMAT_MARKER) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format); + readCounter -= BYTES_PER_SENSOR; + if (mFlushSensorEnabled != -1) { + mFlushBatchSet = 1; + } + mDataMarkerDetected = 1; + mSkipReadEvents = 1; + } + else if (data_format & DATA_FORMAT_EMPTY_MARKER) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format); + readCounter -= BYTES_PER_SENSOR; + if (mFlushSensorEnabled != -1) { + mFlushBatchSet = 1; + } + mEmptyDataMarkerDetected = 1; + mSkipReadEvents = 1; + } + else if (data_format == DATA_FORMAT_QUAT) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "QUAT DETECTED:0x%x", data_format); + if (readCounter >= BYTES_QUAT_DATA) { + mCachedQuaternionData[0] = *((int *) (rdata + 4)); + mCachedQuaternionData[1] = *((int *) (rdata + 8)); + mCachedQuaternionData[2] = *((int *) (rdata + 12)); + rdata += QUAT_ONLY_LAST_PACKET_OFFSET; + mQuatSensorTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_QUAT; + readCounter -= BYTES_QUAT_DATA; + } + else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_6_AXIS) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "6AXIS DETECTED:0x%x", data_format); + if (readCounter >= BYTES_QUAT_DATA) { + mCached6AxisQuaternionData[0] = *((int *) (rdata + 4)); + mCached6AxisQuaternionData[1] = *((int *) (rdata + 8)); + mCached6AxisQuaternionData[2] = *((int *) (rdata + 12)); + rdata += QUAT_ONLY_LAST_PACKET_OFFSET; + mQuatSensorTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_6_AXIS; + readCounter -= BYTES_QUAT_DATA; + } + else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_PED_QUAT) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PED QUAT DETECTED:0x%x", data_format); + if (readCounter >= BYTES_PER_SENSOR_PACKET) { + mCachedPedQuaternionData[0] = *((short *) (rdata + 2)); + mCachedPedQuaternionData[1] = *((short *) (rdata + 4)); + mCachedPedQuaternionData[2] = *((short *) (rdata + 6)); + rdata += BYTES_PER_SENSOR; + mQuatSensorTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_PED_QUAT; + readCounter -= BYTES_PER_SENSOR_PACKET; + } + else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_PED_STANDALONE) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STANDALONE STEP DETECTED:0x%x", data_format); + if (readCounter >= BYTES_PER_SENSOR_PACKET) { + rdata += BYTES_PER_SENSOR; + mStepSensorTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_PED_STANDALONE; + readCounter -= BYTES_PER_SENSOR_PACKET; + mPedUpdate |= data_format; + } + else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_GYRO) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "GYRO DETECTED:0x%x", data_format); + if (readCounter >= BYTES_PER_SENSOR_PACKET) { + mCachedGyroData[0] = *((short *) (rdata + 2)); + mCachedGyroData[1] = *((short *) (rdata + 4)); + mCachedGyroData[2] = *((short *) (rdata + 6)); + rdata += BYTES_PER_SENSOR; + mGyroSensorTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_GYRO; + readCounter -= BYTES_PER_SENSOR_PACKET; + } else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_ACCEL) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "ACCEL DETECTED:0x%x", data_format); + if (readCounter >= BYTES_PER_SENSOR_PACKET) { + mCachedAccelData[0] = *((short *) (rdata + 2)); + mCachedAccelData[1] = *((short *) (rdata + 4)); + mCachedAccelData[2] = *((short *) (rdata + 6)); + rdata += BYTES_PER_SENSOR; + mAccelSensorTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_ACCEL; + readCounter -= BYTES_PER_SENSOR_PACKET; + } + else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_COMPASS) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "COMPASS DETECTED:0x%x", data_format); + if (readCounter >= BYTES_PER_SENSOR_PACKET) { + if (mCompassSensor->isIntegrated()) { + mCachedCompassData[0] = *((short *) (rdata + 2)); + mCachedCompassData[1] = *((short *) (rdata + 4)); + mCachedCompassData[2] = *((short *) (rdata + 6)); + rdata += BYTES_PER_SENSOR; + mCompassTimestamp = *((long long*) (rdata)); + mask |= DATA_FORMAT_COMPASS; + readCounter -= BYTES_PER_SENSOR_PACKET; + } + } + else { + doneFlag = 1; + } + } + else if (data_format == DATA_FORMAT_PRESSURE) { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PRESSURE DETECTED:0x%x", data_format); + if (readCounter >= BYTES_QUAT_DATA) { + if (mPressureSensor->isIntegrated()) { + mCachedPressureData = + ((*((short *)(rdata + 4))) << 16) + + (*((unsigned short *) (rdata + 6))); + rdata += BYTES_PER_SENSOR; + mPressureTimestamp = *((long long*) (rdata)); + if (mCachedPressureData != 0) { + mask |= DATA_FORMAT_PRESSURE; + } + readCounter -= BYTES_PER_SENSOR_PACKET; + } + } else{ + doneFlag = 1; + } + } + + if(doneFlag == 0) { + rdata += BYTES_PER_SENSOR; + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is zero, readCounter=%d", (int)readCounter); + } + else { + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is set, readCounter=%d", (int)readCounter); + } + + /* read ahead and store left over data if any */ + if (readCounter != 0) { + int currentBufferCounter = 0; + LOGV_IF(0, "Not enough data readCounter=%d, expected nbyte=%d, rsize=%d", (int)readCounter, nbyte, (int)rsize); + memset(mLeftOverBuffer, 0, sizeof(mLeftOverBuffer)); + memcpy(mLeftOverBuffer, rdata, readCounter); + LOGV_IF(0, + "HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, " + "%d, %d, %d,%d, %d, %d, %d\n", + mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3], + mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7], + mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11], + mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]); + + mLeftOverBufferSize = readCounter; + readCounter = 0; + LOGV_IF(0, "Stored number of bytes:%d", mLeftOverBufferSize); + } else { + /* reset count since this is the last packet for the data set */ + readCounter = 0; + mLeftOverBufferSize = 0; + } + + /* handle data read */ + if (mask & DATA_FORMAT_GYRO) { + /* batch mode does not batch temperature */ + /* disable temperature read */ + if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) { + // send down temperature every 0.5 seconds + // with timestamp measured in "driver" layer + if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) { + mTempCurrentTime = mGyroSensorTimestamp; + long long temperature[2]; + if(inv_read_temperature(temperature) == 0) { + LOGV_IF(INPUT_DATA, + "HAL:input 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_mpl_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 + } + } + mPendingMask |= 1 << Gyro; + mPendingMask |= 1 << RawGyro; + + inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld", + mCachedGyroData[0], mCachedGyroData[1], + mCachedGyroData[2], mGyroSensorTimestamp); + latestTimestamp = mGyroSensorTimestamp; + } + + if (mask & DATA_FORMAT_ACCEL) { + mPendingMask |= 1 << Accelerometer; + inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld", + mCachedAccelData[0], mCachedAccelData[1], + mCachedAccelData[2], mAccelSensorTimestamp); + /* remember inital 6 axis quaternion */ + inv_time_t tempTimestamp; + inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp); + if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 && + mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) { + mInitial6QuatValueAvailable = 1; + LOGV_IF(INPUT_DATA && ENG_VERBOSE, + "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld", + mInitial6QuatValue[0], mInitial6QuatValue[1], + mInitial6QuatValue[2], mInitial6QuatValue[3]); + } + latestTimestamp = mAccelSensorTimestamp; + } + + if ((mask & DATA_FORMAT_COMPASS) && mCompassSensor->isIntegrated()) { + int status = 0; + if (mCompassSensor->providesCalibration()) { + status = mCompassSensor->getAccuracy(); + status |= INV_CALIBRATED; + } + inv_build_compass(mCachedCompassData, status, + mCompassTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], + mCachedCompassData[2], mCompassTimestamp); + latestTimestamp = mCompassTimestamp; + } + + if (mask & DATA_FORMAT_QUAT) { + /* if bias was applied to DMP bias, + set status bits to disable gyro bias cal */ + int status = 0; + if (mGyroBiasApplied == true) { + LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used"); + status |= INV_BIAS_APPLIED; + } + status |= INV_CALIBRATED | INV_QUAT_3AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */ + inv_build_quat(mCachedQuaternionData, + status, + mQuatSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_quat-3x: %+8ld %+8ld %+8ld - %lld", + mCachedQuaternionData[0], mCachedQuaternionData[1], + mCachedQuaternionData[2], + mQuatSensorTimestamp); + latestTimestamp = mQuatSensorTimestamp; + } + + if (mask & DATA_FORMAT_6_AXIS) { + /* if bias was applied to DMP bias, + set status bits to disable gyro bias cal */ + int status = 0; + if (mGyroBiasApplied == true) { + LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used"); + status |= INV_QUAT_6AXIS; + } + status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */ + inv_build_quat(mCached6AxisQuaternionData, + status, + mQuatSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld", + mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1], + mCached6AxisQuaternionData[2], mQuatSensorTimestamp); + latestTimestamp = mQuatSensorTimestamp; + } + + if (mask & DATA_FORMAT_PED_QUAT) { + /* if bias was applied to DMP bias, + set status bits to disable gyro bias cal */ + int status = 0; + if (mGyroBiasApplied == true) { + LOGV_IF(INPUT_DATA && ENG_VERBOSE, + "HAL:input dmp bias is used"); + status |= INV_QUAT_6AXIS; + } + status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */ + mCachedPedQuaternionData[0] = mCachedPedQuaternionData[0] << 16; + mCachedPedQuaternionData[1] = mCachedPedQuaternionData[1] << 16; + mCachedPedQuaternionData[2] = mCachedPedQuaternionData[2] << 16; + inv_build_quat(mCachedPedQuaternionData, + status, + mQuatSensorTimestamp); + + LOGV_IF(INPUT_DATA, + "HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld", + mCachedPedQuaternionData[0], mCachedPedQuaternionData[1], + mCachedPedQuaternionData[2], mQuatSensorTimestamp); + latestTimestamp = mQuatSensorTimestamp; + } + + if ((mask & DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) { + int status = 0; + if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) { + latestTimestamp = mPressureTimestamp; + mPressureUpdate = 1; + inv_build_pressure(mCachedPressureData, + status, + mPressureTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_pressure: %+8ld - %lld", + mCachedPressureData, mPressureTimestamp); + } + } + + /* take the latest timestamp */ + if (mask & DATA_FORMAT_STEP) { + /* work around driver output duplicate step detector bit */ + if (latestTimestamp > mStepSensorTimestamp) { + mStepSensorTimestamp = latestTimestamp; + LOGV_IF(INPUT_DATA, + "HAL:input build step: 1 - %lld", mStepSensorTimestamp); + } else { + mPedUpdate = 0; + } + } + } //while end +} + +int MPLSensor::checkValidHeader(unsigned short data_format) +{ + if(data_format & DATA_FORMAT_STEP) { + data_format &= (~DATA_FORMAT_STEP); + } + + LOGV_IF(ENG_VERBOSE && INPUT_DATA, "check data_format=%x", data_format); + + if ((data_format == DATA_FORMAT_PED_STANDALONE) || + (data_format == DATA_FORMAT_PED_QUAT) || + (data_format == DATA_FORMAT_6_AXIS) || + (data_format == DATA_FORMAT_QUAT) || + (data_format == DATA_FORMAT_COMPASS) || + (data_format == DATA_FORMAT_GYRO) || + (data_format == DATA_FORMAT_ACCEL) || + (data_format == DATA_FORMAT_PRESSURE) || + (data_format == DATA_FORMAT_EMPTY_MARKER) || + (data_format == DATA_FORMAT_MARKER)) + return 1; + else { + LOGV_IF(ENG_VERBOSE, "bad data_format = %x", data_format); + return 0; + } +} + +/* use for both MPUxxxx and third party compass */ +void MPLSensor::buildCompassEvent(void) +{ + VHANDLER_LOG; + + int done = 0; + + // pthread_mutex_lock(&mMplMutex); + // pthread_mutex_lock(&mHALMutex); + + done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); + if(mCompassSensor->isYasCompass()) { + if (mCompassSensor->checkCoilsReset() == 1) { + //Reset relevant compass settings + resetCompass(); + } + } + if (done > 0) { + int status = 0; + if (mCompassSensor->providesCalibration()) { + status = mCompassSensor->getAccuracy(); + status |= INV_CALIBRATED; + } + if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { + inv_build_compass(mCachedCompassData, status, + mCompassTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], + mCachedCompassData[2], mCompassTimestamp); + } + } + + // pthread_mutex_unlock(&mMplMutex); + // pthread_mutex_unlock(&mHALMutex); +} + +int MPLSensor::resetCompass(void) +{ + VFUNC_LOG; + + //Reset compass cal if enabled + if (mMplFeatureActiveMask & INV_COMPASS_CAL) { + LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal"); + inv_init_vector_compass_cal(); + } + + //Reset compass fit if enabled + if (mMplFeatureActiveMask & INV_COMPASS_FIT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit"); + inv_init_compass_fit(); + } + + return 0; +} + +int MPLSensor::getFd(void) const +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "getFd returning %d", iio_fd); + return iio_fd; +} + +int MPLSensor::getAccelFd(void) const +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "getAccelFd returning %d", accel_fd); + return accel_fd; +} + +int MPLSensor::getCompassFd(void) const +{ + VFUNC_LOG; + int fd = mCompassSensor->getFd(); + LOGV_IF(EXTRA_VERBOSE, "getCompassFd returning %d", fd); + return fd; +} + +int MPLSensor::turnOffAccelFifo(void) +{ + VFUNC_LOG; + int i, res = 0, tempFd; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.accel_fifo_enable, getTimestamp()); + res += write_sysfs_int(mpu.accel_fifo_enable, 0); + return res; +} + +int MPLSensor::turnOffGyroFifo(void) +{ + VFUNC_LOG; + int i, res = 0, tempFd; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.gyro_fifo_enable, getTimestamp()); + res += write_sysfs_int(mpu.gyro_fifo_enable, 0); + return res; +} + +int MPLSensor::enableDmpOrientation(int en) +{ + VFUNC_LOG; + int res = 0; + int enabled_sensors = mEnabled; + + if (isMpuNonDmp()) + return res; + + // reset master enable + res = masterEnable(0); + if (res < 0) + return res; + + 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 + return res; + } + + // enable accel engine + res = enableAccel(1); + if (res < 0) + return res; + + // disable accel FIFO + if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { + res = turnOffAccelFifo(); + if (res < 0) + return res; + } + + if (!mEnabled){ + 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, en) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + } + + mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION; + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + } else { + mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION; + + if (mFeatureActiveMask == 0) { + // disable accel engine + if (!(mLocalSensorMask & mMasterSensorMask + & INV_THREE_AXIS_ACCEL)) { + res = enableAccel(0); + if (res < 0) + return res; + } + } + + if (mEnabled){ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + } + LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); + } + + if ((res = computeAndSetDmpState()) < 0) + return res; + + if (en || mEnabled || mFeatureActiveMask) { + res = masterEnable(1); + } + return res; +} + +int MPLSensor::openDmpOrientFd(void) +{ + VFUNC_LOG; + + if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) { + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP display orientation disabled or file desc opened"); + return 0; + } + + 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(void) +{ + 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; + } else { + if (fscanf(fp, "%d\n", &screen_orientation) < 0 || fclose(fp) < 0) + { + LOGE("HAL:cannot write event_display_orientation"); + } + } + + int numEventReceived = 0; + + if (mDmpOrientationEnabled && count > 0) { + sensors_event_t temp; + + temp.acceleration.x = 0; + temp.acceleration.y = 0; + temp.acceleration.z = 0; + temp.version = sizeof(sensors_event_t); + temp.sensor = ID_SO; + temp.acceleration.status + = SENSOR_STATUS_UNRELIABLE; +#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(void) +{ + VFUNC_LOG; + + LOGV_IF(EXTRA_VERBOSE, "getDmpOrientFd returning %d", dmp_orient_fd); + return dmp_orient_fd; + +} + +int MPLSensor::checkDMPOrientation(void) +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0); +} + +int MPLSensor::getDmpRate(int64_t *wanted) +{ + VFUNC_LOG; + + // set DMP output rate to FIFO + if(mDmpOn == 1) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / *wanted), mpu.three_axis_q_rate, + getTimestamp()); + write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / *wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP three axis rate %.2f Hz", 1000000000.f / *wanted); + if (mFeatureActiveMask & INV_DMP_BATCH_MODE) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / *wanted), mpu.six_axis_q_rate, + getTimestamp()); + write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / *wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP six axis rate %.2f Hz", 1000000000.f / *wanted); + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / *wanted), mpu.ped_q_rate, + getTimestamp()); + write_sysfs_int(mpu.ped_q_rate, 1000000000.f / *wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP ped quaternion 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(void) +{ + VFUNC_LOG; + return mPollTime; +} + +int MPLSensor::getStepCountPollTime(void) +{ + VFUNC_LOG; + /* clamped to 1ms? as spec, still rather large */ + return 1000; +} + +bool MPLSensor::hasStepCountPendingEvents(void) +{ + VFUNC_LOG; + if (mDmpStepCountEnabled) { + struct timespec t_now; + int64_t interval = 0; + + clock_gettime(CLOCK_MONOTONIC, &t_now); + interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) - + (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec)); + + if (interval < mStepCountPollTime) { + LOGV_IF(0, + "Step Count interval elapsed: %lld, triggered: %lld", + interval, mStepCountPollTime); + return false; + } else { + clock_gettime(CLOCK_MONOTONIC, &mt_pre); + LOGV_IF(0, "Step Count previous time: %ld ms", + mt_pre.tv_nsec / 1000); + return true; + } + } + return false; +} + +bool MPLSensor::hasPendingEvents(void) const +{ + VFUNC_LOG; + // if we are using the polling workaround, force the main + // loop to check for data every time + return (mPollTime != -1); +} + +int MPLSensor::inv_read_temperature(long long *data) +{ + VHANDLER_LOG; + + int count = 0; + char raw_buf[40]; + long raw = 0; + + long long timestamp = 0; + + memset(raw_buf, 0, sizeof(raw_buf)); + count = read_attribute_sensor(gyro_temperature_fd, raw_buf, + sizeof(raw_buf)); + if(count < 1) { + LOGE("HAL:error reading gyro temperature"); + return -1; + } + + count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp); + + if(count < 0) { + return -1; + } + + LOGV_IF(ENG_VERBOSE && INPUT_DATA, + "HAL:temperature raw = %ld, timestamp = %lld, count = %d", + raw, timestamp, count); + data[0] = raw; + data[1] = timestamp; + + return 0; +} + +int MPLSensor::inv_read_dmp_state(int fd) +{ + VFUNC_LOG; + + if(fd < 0) + return -1; + + int count = 0; + char raw_buf[10]; + short raw = 0; + + memset(raw_buf, 0, sizeof(raw_buf)); + count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); + if(count < 1) { + LOGE("HAL:error reading dmp state"); + close(fd); + return -1; + } + count = sscanf(raw_buf, "%hd", &raw); + if(count < 0) { + LOGE("HAL:dmp state data is invalid"); + close(fd); + return -1; + } + LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count); + close(fd); + return (int)raw; +} + +int MPLSensor::inv_read_sensor_bias(int fd, long *data) +{ + VFUNC_LOG; + + if(fd == -1) { + return -1; + } + + char buf[50]; + char x[15], y[15], z[15]; + + memset(buf, 0, sizeof(buf)); + int count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + LOGE("HAL:Error reading gyro bias"); + return -1; + } + 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)", + 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)", + 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(sBaseSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { + LOGE("HAL:sensor list too small, not populating."); + return -(sizeof(sBaseSensorList) / sizeof(sensor_t)); + } + + /* fill in the base values */ + memcpy(list, sBaseSensorList, + sizeof (struct sensor_t) * (sizeof(sBaseSensorList) / sizeof(sensor_t))); + + /* first add gyro, accel and compass to the list */ + + /* fill in gyro/accel values */ + if(chip_ID == NULL) { + LOGE("HAL:Can not get gyro/accel id"); + } + fillGyro(chip_ID, list); + fillAccel(chip_ID, list); + + // TODO: need fixes for unified HAL and 3rd-party solution + mCompassSensor->fillList(&list[MagneticField]); + mCompassSensor->fillList(&list[RawMagneticField]); + + if (mPressureSensor != NULL) { + mPressureSensor->fillList(&list[Pressure]); + } + + if(1) { + numsensors = (sizeof(sBaseSensorList) / sizeof(sensor_t)); + /* all sensors will be added to the list + fill in orientation values */ + fillOrientation(list); + /* fill in rotation vector values */ + fillRV(list); + /* fill in game rotation vector values */ + fillGRV(list); + /* fill in gravity values */ + fillGravity(list); + /* fill in Linear accel values */ + fillLinearAccel(list); + /* fill in Significant motion values */ + fillSignificantMotion(list); +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + /* fill in screen orientation values */ + fillScreenOrientation(list); +#endif + } else { + /* no 9-axis sensors, zero fill that part of the list */ + numsensors = 3; + memset(list + 3, 0, 4 * sizeof(struct sensor_t)); + } + + return numsensors; +} + +void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) +{ + VFUNC_LOG; + + if (accel) { + if(accel != NULL && strcmp(accel, "BMA250") == 0) { + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; + return; + } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) { + 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; + return; + } else if (accel != NULL && strcmp(accel, "MPU6515") == 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; + 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; + 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; + return; + } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) { + list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE; + list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU9150_POWER; + list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY; + return; + } else if (accel != NULL && strcmp(accel, "MPU9250") == 0) { + list[Accelerometer].maxRange = ACCEL_MPU9250_RANGE; + list[Accelerometer].resolution = ACCEL_MPU9250_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU9250_POWER; + list[Accelerometer].minDelay = ACCEL_MPU9250_MINDELAY; + return; + } else if (accel != NULL && strcmp(accel, "MPU9350") == 0) { + list[Accelerometer].maxRange = ACCEL_MPU9350_RANGE; + list[Accelerometer].resolution = ACCEL_MPU9350_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU9350_POWER; + list[Accelerometer].minDelay = ACCEL_MPU9350_MINDELAY; + return; + } 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; + list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; + return; + } + } + + LOGE("HAL:unknown accel id %s -- " + "params default to mpu6515 and might be wrong.", + accel); + list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; + list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU6500_POWER; + list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; +} + +void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) +{ + VFUNC_LOG; + + if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) { + list[Gyro].maxRange = GYRO_MPU3050_RANGE; + list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; + list[Gyro].power = GYRO_MPU3050_POWER; + list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; + } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) { + 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; + } else if( gyro != NULL && strcmp(gyro, "MPU6515") == 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; + } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) { + list[Gyro].maxRange = GYRO_MPU9150_RANGE; + list[Gyro].resolution = GYRO_MPU9150_RESOLUTION; + list[Gyro].power = GYRO_MPU9150_POWER; + list[Gyro].minDelay = GYRO_MPU9150_MINDELAY; + } else if( gyro != NULL && strcmp(gyro, "MPU9250") == 0) { + list[Gyro].maxRange = GYRO_MPU9250_RANGE; + list[Gyro].resolution = GYRO_MPU9250_RESOLUTION; + list[Gyro].power = GYRO_MPU9250_POWER; + list[Gyro].minDelay = GYRO_MPU9250_MINDELAY; + } else if( gyro != NULL && strcmp(gyro, "MPU9350") == 0) { + list[Gyro].maxRange = GYRO_MPU9350_RANGE; + list[Gyro].resolution = GYRO_MPU9350_RESOLUTION; + list[Gyro].power = GYRO_MPU9350_POWER; + list[Gyro].minDelay = GYRO_MPU9350_MINDELAY; + } else { + LOGE("HAL:unknown gyro id -- gyro params will be wrong."); + LOGE("HAL:default to use mpu6515 params"); + list[Gyro].maxRange = GYRO_MPU6500_RANGE; + list[Gyro].resolution = GYRO_MPU6500_RESOLUTION; + list[Gyro].power = GYRO_MPU6500_POWER; + list[Gyro].minDelay = GYRO_MPU6500_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; +} + +/* fillRV depends on values of gyro, accel and compass in the list */ +void MPLSensor::fillRV(struct sensor_t *list) +{ + VFUNC_LOG; + + /* compute power on the fly */ + list[RotationVector].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[RotationVector].resolution = .00001; + list[RotationVector].maxRange = 1.0; + list[RotationVector].minDelay = 5000; + + return; +} + +/* fillGMRV depends on values of accel and mag in the list */ +void MPLSensor::fillGMRV(struct sensor_t *list) +{ + VFUNC_LOG; + + /* compute power on the fly */ + list[GeomagneticRotationVector].power = list[Accelerometer].power + + list[MagneticField].power; + list[GeomagneticRotationVector].resolution = .00001; + list[GeomagneticRotationVector].maxRange = 1.0; + list[GeomagneticRotationVector].minDelay = 5000; + + return; +} + +/* fillGRV depends on values of gyro and accel in the list */ +void MPLSensor::fillGRV(struct sensor_t *list) +{ + VFUNC_LOG; + + /* compute power on the fly */ + list[GameRotationVector].power = list[Gyro].power + + list[Accelerometer].power; + list[GameRotationVector].resolution = .00001; + list[GameRotationVector].maxRange = 1.0; + list[GameRotationVector].minDelay = 5000; + + return; +} + +void MPLSensor::fillOrientation(struct sensor_t *list) +{ + VFUNC_LOG; + + list[Orientation].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[Orientation].resolution = .00001; + list[Orientation].maxRange = 360.0; + list[Orientation].minDelay = 5000; + + return; +} + +void MPLSensor::fillGravity( struct sensor_t *list) +{ + VFUNC_LOG; + + list[Gravity].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[Gravity].resolution = .00001; + list[Gravity].maxRange = 9.81; + list[Gravity].minDelay = 5000; + + return; +} + +void MPLSensor::fillLinearAccel(struct sensor_t *list) +{ + VFUNC_LOG; + + list[LinearAccel].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[LinearAccel].resolution = list[Accelerometer].resolution; + list[LinearAccel].maxRange = list[Accelerometer].maxRange; + list[LinearAccel].minDelay = 5000; + + return; +} + +void MPLSensor::fillSignificantMotion(struct sensor_t *list) +{ + VFUNC_LOG; + + list[SignificantMotion].power = list[Accelerometer].power; + list[SignificantMotion].resolution = 1; + list[SignificantMotion].maxRange = 1; + list[SignificantMotion].minDelay = -1; +} + +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION +void MPLSensor::fillScreenOrientation(struct sensor_t *list) +{ + VFUNC_LOG; + + list[NumSensors].power = list[Accelerometer].power; + list[NumSensors].resolution = 1; + list[NumSensors].maxRange = 3; + list[NumSensors].minDelay = 0; +} +#endif + +int MPLSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + unsigned char i = 0; + char sysfs_path[MAX_SYSFS_NAME_LEN]; + char tbuf[2]; + char *sptr; + char **dptr; + int num; + + memset(sysfs_path, 0, sizeof(sysfs_path)); + + 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; + memset(sptr, 0, sizeof(sptr)); + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < MAX_SYSFS_ATTRB); + } else { + LOGE("HAL:couldn't alloc mem for sysfs paths"); + return -1; + } + + // get proper (in absolute) IIO path & build MPU's sysfs paths + inv_get_sysfs_path(sysfs_path); + + memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path)); + sprintf(mpu.key, "%s%s", sysfs_path, "/key"); + sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); + sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length"); + sprintf(mpu.master_enable, "%s%s", sysfs_path, "/master_enable"); + sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); + + sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, + "/scan_elements/in_timestamp_en"); + sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path, + "/scan_elements/in_timestamp_index"); + sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path, + "/scan_elements/in_timestamp_type"); + + sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware"); + 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.tap_on, "%s%s", sysfs_path, "/tap_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_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); + sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix"); + sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable"); + sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale"); + sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable"); + sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate"); + + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable"); + sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); + sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix"); + sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable"); + sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate"); + +#ifndef THIRD_PARTY_ACCEL //MPU3050 + sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); + + // DMP uses these values + sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias"); + sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias"); + sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias"); + + // MPU uses these values + sprintf(mpu.in_accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset"); + sprintf(mpu.in_accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset"); + sprintf(mpu.in_accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset"); + sprintf(mpu.in_accel_self_test_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale"); +#endif + + // DMP uses these bias values + sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias"); + sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias"); + sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias"); + + // MPU uses these bias values + sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset"); + sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset"); + sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset"); + sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale"); + + sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on + sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate"); + + sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on"); + sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate"); + + sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on"); + sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate"); + + sprintf(mpu.six_axis_q_value, "%s%s", sysfs_path, "/six_axes_q_value"); + + sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on"); + sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on"); + + sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, + "/display_orientation_on"); + sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, + "/event_display_orientation"); + + sprintf(mpu.event_smd, "%s%s", sysfs_path, + "/event_smd"); + sprintf(mpu.smd_enable, "%s%s", sysfs_path, + "/smd_enable"); + sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path, + "/smd_delay_threshold"); + sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path, + "/smd_delay_threshold2"); + sprintf(mpu.smd_threshold, "%s%s", sysfs_path, + "/smd_threshold"); + sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path, + "/batchmode_timeout"); + sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path, + "/batchmode_wake_fifo_full_on"); + sprintf(mpu.flush_batch, "%s%s", sysfs_path, + "/flush_batch"); + sprintf(mpu.pedometer_on, "%s%s", sysfs_path, + "/pedometer_on"); + sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path, + "/pedometer_int_on"); + sprintf(mpu.event_pedometer, "%s%s", sysfs_path, + "/event_pedometer"); + sprintf(mpu.pedometer_steps, "%s%s", sysfs_path, + "/pedometer_steps"); + sprintf(mpu.pedometer_counter, "%s%s", sysfs_path, + "/pedometer_counter"); + sprintf(mpu.motion_lpa_on, "%s%s", sysfs_path, + "/motion_lpa_on"); + return 0; +} + +//DMP support only for MPU6xxx/9xxx +bool MPLSensor::isMpuNonDmp(void) +{ + VFUNC_LOG; + if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) + return true; + else + return false; +} + +int MPLSensor::isLowPowerQuatEnabled(void) +{ + VFUNC_LOG; +#ifdef ENABLE_LP_QUAT_FEAT + return !isMpuNonDmp(); +#else + return 0; +#endif +} + +int MPLSensor::isDmpDisplayOrientationOn(void) +{ + VFUNC_LOG; +#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT + if (isMpuNonDmp()) + return 0; + return 1; +#else + return 0; +#endif +} + +/* these functions can be consolidated +with inv_convert_to_body_with_scale */ +void MPLSensor::getCompassBias() +{ + VFUNC_LOG; + + + long bias[3]; + long compassBias[3]; + unsigned short orient; + signed char orientMtx[9]; + mCompassSensor->getOrientationMatrix(orientMtx); + orient = inv_orientation_matrix_to_scalar(orientMtx); + + /* Get Values from MPL */ + inv_get_compass_bias(bias); + inv_convert_to_body(orient, bias, compassBias); + LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]); + LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]); + long compassSensitivity = inv_get_compass_sensitivity(); + if (compassSensitivity == 0) { + compassSensitivity = mCompassScale; + } + for(int i=0; i<3; i++) { + /* convert to uT */ + float temp = (float) compassSensitivity / (1L << 30); + mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f); + } + + return; +} + +void MPLSensor::getFactoryGyroBias() +{ + VFUNC_LOG; + + /* Get Values from MPL */ + inv_get_gyro_bias(mFactoryGyroBias); + LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]); + mFactoryGyroBiasAvailable = true; + + return; +} + +/* set bias from factory cal file to MPU offset (in chip frame) + x = values store in cal file --> (v/1000 * 2^16 / (2000/250)) + offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale + i.e. self test default scale = 250 + gyro scale default to = 2000 + offset scale = 4 //as spec by hardware + offset = x/2^16 * (8) * (-1) / (4) +*/ +void MPLSensor::setFactoryGyroBias() +{ + VFUNC_LOG; + int scaleRatio = mGyroScale / mGyroSelfTestScale; + int offsetScale = 4; + LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio); + LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale); + + /* Write to Driver */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale), + mpu.in_gyro_x_offset, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_x_offset_fd, + (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0) + { + LOGE("HAL:Error writing to gyro_x_offset"); + return; + } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale), + mpu.in_gyro_y_offset, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_y_offset_fd, + (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0) + { + LOGE("HAL:Error writing to gyro_y_offset"); + return; + } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale), + mpu.in_gyro_z_offset, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_z_offset_fd, + (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0) + { + LOGE("HAL:Error writing to gyro_z_offset"); + return; + } + mFactoryGyroBiasAvailable = false; + LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied"); + + return; +} + +/* these functions can be consolidated +with inv_convert_to_body_with_scale */ +void MPLSensor::getGyroBias() +{ + VFUNC_LOG; + + long *temp = NULL; + long chipBias[3]; + long bias[3]; + unsigned short orient; + + /* Get Values from MPL */ + inv_get_mpl_gyro_bias(mGyroChipBias, temp); + orient = inv_orientation_matrix_to_scalar(mGyroOrientation); + inv_convert_to_body(orient, mGyroChipBias, bias); + LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]); + LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]); + long gyroSensitivity = inv_get_gyro_sensitivity(); + if(gyroSensitivity == 0) { + gyroSensitivity = mGyroScale; + } + + /* scale and convert to rad */ + for(int i=0; i<3; i++) { + float temp = (float) gyroSensitivity / (1L << 30); + mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI); + if (mGyroBias[i] != 0) + mGyroBiasAvailable = true; + } + + return; +} + +void MPLSensor::setGyroZeroBias() +{ + VFUNC_LOG; + + /* Write to Driver */ + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.in_gyro_x_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_x_dmp_bias"); + return; + } + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.in_gyro_y_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_y_dmp_bias"); + return; + } + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.in_gyro_z_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_z_dmp_bias"); + return; + } + LOGV_IF(EXTRA_VERBOSE, "HAL:Zero Gyro DMP Calibrated Bias Applied"); + + return; +} + +void MPLSensor::setGyroBias() +{ + VFUNC_LOG; + + if(mGyroBiasAvailable == false) + return; + + long bias[3]; + long gyroSensitivity = inv_get_gyro_sensitivity(); + + if(gyroSensitivity == 0) { + gyroSensitivity = mGyroScale; + } + + inv_get_gyro_bias_dmp_units(bias); + + /* Write to Driver */ + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)", + bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0) { + LOGE("HAL:Error writing to gyro_x_dmp_bias"); + return; + } + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)", + bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0) { + LOGE("HAL:Error writing to gyro_y_dmp_bias"); + return; + } + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)", + bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0) { + LOGE("HAL:Error writing to gyro_z_dmp_bias"); + return; + } + mGyroBiasApplied = true; + mGyroBiasAvailable = false; + LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied"); + + return; +} + +void MPLSensor::getFactoryAccelBias() +{ + VFUNC_LOG; + + long temp; + + /* Get Values from MPL */ + inv_get_accel_bias(mFactoryAccelBias); + LOGV_IF(ENG_VERBOSE, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]); + mFactoryAccelBiasAvailable = true; + + return; +} + +void MPLSensor::setFactoryAccelBias() +{ + VFUNC_LOG; + + if(mFactoryAccelBiasAvailable == false) + return; + + /* add scaling here - depends on self test parameters */ + int scaleRatio = mAccelScale / mAccelSelfTestScale; + int offsetScale = 16; + long tempBias; + + LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio); + LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale); + + /* Write to Driver */ + tempBias = -mFactoryAccelBias[0] / 65536.f * scaleRatio / offsetScale; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", + tempBias, mpu.in_accel_x_offset, getTimestamp()); + if(write_attribute_sensor_continuous(accel_x_offset_fd, tempBias) < 0) { + LOGE("HAL:Error writing to accel_x_offset"); + return; + } + tempBias = -mFactoryAccelBias[1] / 65536.f * scaleRatio / offsetScale; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", + tempBias, mpu.in_accel_y_offset, getTimestamp()); + if(write_attribute_sensor_continuous(accel_y_offset_fd, tempBias) < 0) { + LOGE("HAL:Error writing to accel_y_offset"); + return; + } + tempBias = -mFactoryAccelBias[2] / 65536.f * scaleRatio / offsetScale; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", + tempBias, mpu.in_accel_z_offset, getTimestamp()); + if(write_attribute_sensor_continuous(accel_z_offset_fd, tempBias) < 0) { + LOGE("HAL:Error writing to accel_z_offset"); + return; + } + mFactoryAccelBiasAvailable = false; + LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied"); + + return; +} + +void MPLSensor::getAccelBias() +{ + VFUNC_LOG; + long temp; + + /* Get Values from MPL */ + inv_get_mpl_accel_bias(mAccelBias, &temp); + LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld", + mAccelBias[0], mAccelBias[1], mAccelBias[2]); + mAccelBiasAvailable = true; + + return; +} + +/* set accel bias obtained from MPL + bias is scaled by 65536 from MPL + DMP expects: bias * 536870912 / 2^30 = bias / 2 (in body frame) +*/ +void MPLSensor::setAccelBias() +{ + VFUNC_LOG; + + if(mAccelBiasAvailable == false) { + LOGV_IF(ENG_VERBOSE, "HAL: setAccelBias - accel bias not available"); + return; + } + + /* write to driver */ + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)", + (long) (mAccelBias[0] / 65536.f / 2), + mpu.in_accel_x_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous( + accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0) { + LOGE("HAL:Error writing to accel_x_dmp_bias"); + return; + } + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)", + (long)(mAccelBias[1] / 65536.f / 2), + mpu.in_accel_y_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous( + accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0) { + LOGE("HAL:Error writing to accel_y_dmp_bias"); + return; + } + LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)", + (long)(mAccelBias[2] / 65536 / 2), + mpu.in_accel_z_dmp_bias, getTimestamp()); + if(write_attribute_sensor_continuous( + accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0) { + LOGE("HAL:Error writing to accel_z_dmp_bias"); + return; + } + + mAccelBiasAvailable = false; + mAccelBiasApplied = true; + LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied"); + + return; +} + +int MPLSensor::isCompassDisabled(void) +{ + VFUNC_LOG; + if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) { + LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used."); + return 1; + } + return 0; +} + +int MPLSensor::checkBatchEnabled(void) +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_BATCH_MODE)? 1:0); +} + +/* precondition: framework disallows this case, ie enable continuous sensor, */ +/* and enable batch sensor */ +/* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */ +/* or any other sensors */ +int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout) +{ + VFUNC_LOG; + + int res = 0; + + if (isMpuNonDmp()) + return res; + + /* Enables batch mode and sets timeout for the given sensor */ + /* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */ + bool dryRun = false; + android::String8 sname; + int what = -1; + int enabled_sensors = mEnabled; + int batchMode = timeout > 0 ? 1 : 0; + + LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE, + "HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld", + handle, flags, period_ns, timeout); + + if(flags & SENSORS_BATCH_DRY_RUN) { + dryRun = true; + LOGI_IF(PROCESS_VERBOSE, + "HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN); + } + + /* check if we can support issuing interrupt before FIFO fills-up */ + /* in a given timeout. */ + if (flags & SENSORS_BATCH_WAKE_UPON_FIFO_FULL) { + LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported"); + return -EINVAL; + } + + getHandle(handle, what, sname); + if(uint32_t(what) >= NumSensors) { + LOGE("HAL:batch sensors %d not found", what); + return -EINVAL; + } + + LOGV_IF(PROCESS_VERBOSE, + "HAL:batch : %llu ns, (%.2f Hz)", period_ns, 1000000000.f / period_ns); + + // limit all rates to reasonable ones */ + if (period_ns < 5000000LL) { + period_ns = 5000000LL; + } + + switch (what) { + case Gyro: + case RawGyro: + case Accelerometer: + case MagneticField: + case RawMagneticField: + case Pressure: + case GameRotationVector: + case StepDetector: + LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle); + break; + default: + if (timeout > 0) { + LOGE("sensor (handle %d) is not supported in batch mode", handle); + return -EINVAL; + } + } + + if(dryRun == true) { + LOGI("HAL: batch Dry Run is complete"); + return 0; + } + + int tempBatch = 0; + if (timeout > 0) { + tempBatch = mBatchEnabled | (1 << what); + } else { + tempBatch = mBatchEnabled & ~(1 << what); + } + + if (!computeBatchSensorMask(mEnabled, tempBatch)) { + batchMode = 0; + } else { + batchMode = 1; + } + + /* get maximum possible bytes to batch per sample */ + /* get minimum delay for each requested sensor */ + ssize_t nBytes = 0; + int64_t wanted = 1000000000LL, ns = 0; + int64_t timeoutInMs = 0; + for (int i = 0; i < NumSensors; i++) { + if (batchMode == 1) { + ns = mBatchDelays[i]; + LOGV_IF(DEBUG_BATCHING && EXTRA_VERBOSE, + "HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns); + // take the min delay ==> max rate + wanted = (ns < wanted) ? ns : wanted; + if (i <= RawMagneticField) { + nBytes += 8; + } + if (i == Pressure) { + nBytes += 6; + } + if ((i == StepDetector) || (i == GameRotationVector)) { + nBytes += 16; + } + } + } + + /* starting from code below, we will modify hardware */ + /* first edit global batch mode mask */ + + if (!timeout) { + mBatchEnabled &= ~(1 << what); + mBatchDelays[what] = 1000000000L; + mDelays[what] = period_ns; + mBatchTimeouts[what] = 100000000000LL; + } else { + mBatchEnabled |= (1 << what); + mBatchDelays[what] = period_ns; + mDelays[what] = period_ns; + mBatchTimeouts[what] = timeout; + } + + // reset master enable + res = masterEnable(0); + if (res < 0) { + return res; + } + + if(((int)mOldBatchEnabledMask != batchMode) || batchMode) { + + /* remember batch mode that is set */ + mOldBatchEnabledMask = batchMode; + + /* For these sensors, switch to different data output */ + int featureMask = computeBatchDataOutput(); + + LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d", + batchMode, featureMask, mEnabled); + if (DEBUG_BATCHING && EXTRA_VERBOSE) { + LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled); + for (int d = 0; d < NumSensors; d++) { + LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld", + mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d], + mBatchDelays[d]); + } + } + + /* take the minimum batchmode timeout */ + if (batchMode == 1) { + int64_t tempTimeout = 100000000000LL; + for (int i = 0; i < NumSensors; i++) { + if ((mEnabled & (1 << i) && mBatchEnabled & (1 << i)) || + (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) { + LOGV_IF(ENG_VERBOSE, "i=%d, timeout=%lld", i, mBatchTimeouts[i]); + ns = mBatchTimeouts[i]; + tempTimeout = (ns < tempTimeout) ? ns : tempTimeout; + } + } + timeout = tempTimeout; + /* Convert ns to millisecond */ + timeoutInMs = timeout / 1000000; + + /* remember last timeout value */ + mBatchTimeoutInMs = timeoutInMs; + } else { + timeoutInMs = 0; + } + + LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE, + "HAL:batch - timeout - timeout=%lld ns, timeoutInMs=%lld, delay=%lld ns", + timeout, timeoutInMs, period_ns); + + /* case for Ped standalone */ + if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) && + (mFeatureActiveMask & INV_DMP_PEDOMETER)) { + LOGI("ID_P only = 0x%x", mBatchEnabled); + enablePedQuaternion(0); + enablePedStandalone(1); + } else { + enablePedStandalone(0); + if (featureMask & INV_DMP_PED_QUATERNION) { + enableLPQuaternion(0); + enablePedQuaternion(1); + } + } + + /* case for Ped Quaternion */ + if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) && + (mEnabled & (1 << GameRotationVector)) && + (mFeatureActiveMask & INV_DMP_PEDOMETER)) { + LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled); + LOGI("ID_P is enabled for batching, PED quat will be automatically enabled"); + enableLPQuaternion(0); + enablePedQuaternion(1); + + wanted = mBatchDelays[GameRotationVector]; + /* set pedq rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / wanted), mpu.ped_q_rate, + getTimestamp()); + write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted); + } else if (!(featureMask & INV_DMP_PED_STANDALONE)){ + LOGV_IF(ENG_VERBOSE, "batch - PedQ Toggle back to normal 6 axis"); + if (mEnabled & (1 << GameRotationVector)) { + enableLPQuaternion(checkLPQRateSupported()); + } + enablePedQuaternion(0); + } else { + enablePedQuaternion(0); + } + + /* case for Ped indicator */ + if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) { + enablePedIndicator(1); + } else { + enablePedIndicator(0); + } + + /* case for Six Axis Quaternion */ + if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) && + (mEnabled & (1 << GameRotationVector))) { + LOGI("GRV = 0x%x", mBatchEnabled); + enableLPQuaternion(0); + enable6AxisQuaternion(1); + if (what == GameRotationVector) { + setInitial6QuatValue(); + } + + wanted = mBatchDelays[GameRotationVector]; + /* set sixaxis rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / wanted), mpu.six_axis_q_rate, + getTimestamp()); + write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted); + } else if (!(featureMask & INV_DMP_PED_QUATERNION)){ + LOGV_IF(ENG_VERBOSE, "batch - 6Axis Toggle back to normal 6 axis"); + if (mEnabled & (1 << GameRotationVector)) { + enableLPQuaternion(checkLPQRateSupported()); + } + enable6AxisQuaternion(0); + } else { + enable6AxisQuaternion(0); + } + + /* TODO: This may make a come back some day */ + /* Not to overflow hardware FIFO if flag is set */ + /*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.batchmode_wake_fifo_full_on, getTimestamp()); + if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) { + LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on"); + } + }*/ + + /* write required timeout to sysfs */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)", + timeoutInMs, mpu.batchmode_timeout, getTimestamp()); + if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) { + LOGE("HAL:ERR can't write batchmode_timeout"); + } + + if (computeAndSetDmpState() < 0) { + LOGE("HAL:ERR can't compute dmp state"); + } + +}//end of batch mode modify + + if (batchMode == 1) { + /* set batch rates */ + if (setBatchDataRates() < 0) { + LOGE("HAL:ERR can't set batch data rates"); + } + } else { + /* reset sensor rate */ + if (resetDataRates() < 0) { + LOGE("HAL:ERR can't reset output rate back to original setting"); + } + } + + // set sensor data interrupt + uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE)); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + !dataInterrupt, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + + if (enabled_sensors || mFeatureActiveMask) { + masterEnable(1); + } + return res; +} + +/* Send empty event when: */ +/* 1. batch mode is not enabled */ +/* 2. no data in HW FIFO */ +/* return status zero if (2) */ +int MPLSensor::flush(int handle) +{ + VFUNC_LOG; + + int res = 0; + int status = 0; + android::String8 sname; + int what = -1; + + getHandle(handle, what, sname); + if (uint32_t(what) >= NumSensors) { + LOGE("HAL:flush - what=%d is invalid", what); + return -EINVAL; + } + + switch (what) { + case Gyro: + case RawGyro: + case Accelerometer: + case MagneticField: + case RawMagneticField: + case Pressure: + case GameRotationVector: + case StepDetector: + LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle); + break; + default: + LOGE("sensor (handle %d) is not supported in batch or flush mode", handle); + return -EINVAL; + } + + if (((what != StepDetector) && (!(mEnabled & (1 << what)))) || + ((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) { + LOGE("HAL: flush - sensor %s not enabled", sname.string()); + return -EINVAL; + } + + if(!(mBatchEnabled & (1 << what))) { + LOGV_IF(PROCESS_VERBOSE, "HAL:flush - batch mode not enabled for sensor %s (handle %d)", sname.string(), handle); + } + + /*write sysfs */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", + mpu.flush_batch, getTimestamp()); + + status = read_sysfs_int(mpu.flush_batch, &res); + + if (status < 0) + return status; + + /* driver returns 0 if FIFO is empty */ + /* ensure we return status zero */ + if (res == 0) { + LOGI("HAL: flush - no data in FIFO"); + status = 0; + } + + mFlushSensorEnabled = handle; + LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushSensorEnabled=%d", mFlushSensorEnabled); + + mFlushBatchSet = 0; + return status; +} + +int MPLSensor::selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask) +{ + VFUNC_LOG; + int res = 0; + + int64_t wanted; + + /* case for Ped Quaternion */ + if (batchMode == 1) { + if ((featureMask & INV_DMP_PED_QUATERNION) && + (mEnabled & (1 << GameRotationVector)) && + (mFeatureActiveMask & INV_DMP_PEDOMETER)) { + enableLPQuaternion(0); + enable6AxisQuaternion(0); + setInitial6QuatValue(); + enablePedQuaternion(1); + + wanted = mBatchDelays[GameRotationVector]; + /* set pedq rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / wanted), mpu.ped_q_rate, + getTimestamp()); + write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted); + } else if ((featureMask & INV_DMP_6AXIS_QUATERNION) && + (mEnabled & (1 << GameRotationVector))) { + enableLPQuaternion(0); + enablePedQuaternion(0); + setInitial6QuatValue(); + enable6AxisQuaternion(1); + + wanted = mBatchDelays[GameRotationVector]; + /* set sixaxis rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / wanted), mpu.six_axis_q_rate, + getTimestamp()); + write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted); + LOGV_IF(PROCESS_VERBOSE, + "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted); + } else { + enablePedQuaternion(0); + enable6AxisQuaternion(0); + } + } else { + if(mEnabled & (1 << GameRotationVector)) { + enablePedQuaternion(0); + enable6AxisQuaternion(0); + enableLPQuaternion(checkLPQRateSupported()); + } + else { + enablePedQuaternion(0); + enable6AxisQuaternion(0); + } + } + + return res; +} + +/* +Select Quaternion and Options for Batching + + ID_P ID_GRV HW Batch Type +a 1 1 1 PedQ, Ped Indicator, HW +b 1 1 0 PedQ +c 1 0 1 Ped Indicator, HW +d 1 0 0 Ped Standalone, Ped Indicator +e 0 1 1 6Q, HW +f 0 1 0 6Q +g 0 0 1 HW +h 0 0 0 LPQ <defualt> +*/ +int MPLSensor::computeBatchDataOutput() +{ + VFUNC_LOG; + + int featureMask = 0; + if (mBatchEnabled == 0) + return 0;//h + + uint32_t hardwareSensorMask = (1 << Gyro) + | (1 << RawGyro) + | (1 << Accelerometer) + | (1 << MagneticField) + | (1 << RawMagneticField) + | (1 << Pressure); + LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x", + hardwareSensorMask, mBatchEnabled); + + if (mBatchEnabled & (1 << StepDetector)) { + if (mBatchEnabled & (1 << GameRotationVector)) { + if ((mBatchEnabled & hardwareSensorMask)) { + featureMask |= INV_DMP_6AXIS_QUATERNION;//a + featureMask |= INV_DMP_PED_INDICATOR; +//LOGE("batch output: a"); + } else { + featureMask |= INV_DMP_PED_QUATERNION; //b + featureMask |= INV_DMP_PED_INDICATOR; //always piggy back a bit +//LOGE("batch output: b"); + } + } else { + if (mBatchEnabled & hardwareSensorMask) { + featureMask |= INV_DMP_PED_INDICATOR; //c +//LOGE("batch output: c"); + } else { + featureMask |= INV_DMP_PED_STANDALONE; //d + featureMask |= INV_DMP_PED_INDICATOR; //required for standalone +//LOGE("batch output: d"); + } + } + } else if (mBatchEnabled & (1 << GameRotationVector)) { + featureMask |= INV_DMP_6AXIS_QUATERNION; //e,f +//LOGE("batch output: e,f"); + } else { + LOGV_IF(ENG_VERBOSE, + "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask); +//LOGE("batch output: g"); + return 0; //g + } + + LOGV_IF(ENG_VERBOSE, + "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask); + return featureMask; +} + +int MPLSensor::getDmpPedometerFd() +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "getDmpPedometerFd returning %d", dmp_pedometer_fd); + return dmp_pedometer_fd; +} + +/* @param [in] : outputType = 1 --event is from PED_Q */ +/* outputType = 0 --event is from ID_SC, ID_P */ +int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count, + int32_t id, int outputType) +{ + VFUNC_LOG; + + int res = 0; + char dummy[4]; + + int numEventReceived = 0; + int update = 0; + + LOGI_IF(0, "HAL: Read Pedometer Event ID=%d", id); + switch (id) { + case ID_P: + if (mDmpPedometerEnabled && count > 0) { + /* Handles return event */ + LOGI("HAL: Step detected"); + update = sdHandler(&mSdEvents); + } + + if (update && count > 0) { + *data++ = mSdEvents; + count--; + numEventReceived++; + } + break; + case ID_SC: + FILE *fp; + uint64_t stepCount; + uint64_t stepCountTs; + + if (mDmpStepCountEnabled && count > 0) { + fp = fopen(mpu.pedometer_steps, "r"); + if (fp == NULL) { + LOGE("HAL:cannot open pedometer_steps"); + } else { + if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) { + LOGE("HAL:cannot read pedometer_steps"); + return 0; + } + } + + /* return event onChange only */ + if (stepCount == mLastStepCount) { + return 0; + } + + mLastStepCount = stepCount; + + /* Read step count timestamp */ + fp = fopen(mpu.pedometer_counter, "r"); + if (fp == NULL) { + LOGE("HAL:cannot open pedometer_counter"); + } else{ + if (fscanf(fp, "%lld\n", &stepCountTs) < 0 || fclose(fp) < 0) { + LOGE("HAL:cannot read pedometer_counter"); + return 0; + } + } + mScEvents.timestamp = stepCountTs; + + /* Handles return event */ + update = scHandler(&mScEvents); + } + + if (update && count > 0) { + *data++ = mScEvents; + count--; + numEventReceived++; + } + break; + } + + if (!outputType) { + // read dummy data per driver's request + // only required if actual irq is issued + read(dmp_pedometer_fd, dummy, 4); + } else { + return 1; + } + + return numEventReceived; +} + +int MPLSensor::getDmpSignificantMotionFd() +{ + VFUNC_LOG; + + LOGV_IF(EXTRA_VERBOSE, "getDmpSignificantMotionFd returning %d", + dmp_sign_motion_fd); + return dmp_sign_motion_fd; +} + +int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) +{ + VFUNC_LOG; + + int res = 0; + char dummy[4]; + int significantMotion; + FILE *fp; + int sensors = mEnabled; + int numEventReceived = 0; + int update = 0; + + /* Technically this step is not necessary for now */ + /* In the future, we may have meaningful values */ + fp = fopen(mpu.event_smd, "r"); + if (fp == NULL) { + LOGE("HAL:cannot open event_smd"); + return 0; + } else { + if (fscanf(fp, "%d\n", &significantMotion) < 0 || fclose(fp) < 0) { + LOGE("HAL:cannot read event_smd"); + } + } + + if(mDmpSignificantMotionEnabled && count > 0) { + /* By implementation, smd is disabled once an event is triggered */ + sensors_event_t temp; + + /* Handles return event */ + LOGI("HAL: SMD detected"); + int update = smHandler(&mSmEvents); + if (update && count > 0) { + *data++ = mSmEvents; + count--; + numEventReceived++; + + /* reset smd state */ + mDmpSignificantMotionEnabled = 0; + mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION; + } + } + + // read dummy data per driver's request + read(dmp_sign_motion_fd, dummy, 4); + + return numEventReceived; +} + +int MPLSensor::enableDmpSignificantMotion(int en) +{ + VFUNC_LOG; + + int res = 0; + int enabled_sensors = mEnabled; + + if (isMpuNonDmp()) + return res; + + // reset master enable + res = masterEnable(0); + if (res < 0) + return res; + + //Toggle significant montion detection + if(en) { + LOGV_IF(ENG_VERBOSE, "HAL:Enabling Significant Motion"); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.smd_enable, getTimestamp()); + if (write_sysfs_int(mpu.smd_enable, 1) < 0) { + LOGE("HAL:ERR can't write DMP smd_enable"); + res = -1; //Indicate an err + } + mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION; + } + else { + LOGV_IF(ENG_VERBOSE, "HAL:Disabling Significant Motion"); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.smd_enable, getTimestamp()); + if (write_sysfs_int(mpu.smd_enable, 0) < 0) { + LOGE("HAL:ERR write DMP smd_enable"); + } + mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION; + } + + if ((res = setDmpFeature(en)) < 0) + return res; + + if ((res = computeAndSetDmpState()) < 0) + return res; + + if(en || enabled_sensors || mFeatureActiveMask) { + res = masterEnable(1); + } + return res; +} + +void MPLSensor::setInitial6QuatValue() +{ + VFUNC_LOG; + + if (!mInitial6QuatValueAvailable) + return; + + /* convert to unsigned char array */ + size_t length = 16; + unsigned char quat[16]; + convert_long_to_hex_char(mInitial6QuatValue, quat, 4); + + /* write to sysfs */ + LOGV_IF(EXTRA_VERBOSE, "HAL:six axis q value: %s", mpu.six_axis_q_value); + FILE* fptr = fopen(mpu.six_axis_q_value, "w"); + if(fptr == NULL) { + LOGE("HAL:could not open six_axis_q_value"); + } else { + if (fwrite(quat, 1, length, fptr) != length || fclose(fptr) < 0) { + LOGE("HAL:write six axis q value failed"); + } else { + mInitial6QuatValueAvailable = 0; + } + } + + return; +} +int MPLSensor::writeSignificantMotionParams(bool toggleEnable, + uint32_t delayThreshold1, + uint32_t delayThreshold2, + uint32_t motionThreshold) +{ + VFUNC_LOG; + + int res = 0; + + // Turn off enable + if (toggleEnable) { + masterEnable(0); + } + + // Write supplied values + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + delayThreshold1, mpu.smd_delay_threshold, getTimestamp()); + res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1); + if (res == 0) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + delayThreshold2, mpu.smd_delay_threshold2, getTimestamp()); + res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2); + } + if (res == 0) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + motionThreshold, mpu.smd_threshold, getTimestamp()); + res = write_sysfs_int(mpu.smd_threshold, motionThreshold); + } + + // Turn on enable + if (toggleEnable) { + masterEnable(1); + } + return res; +} + +/* set batch data rate */ +/* this function should be optimized */ +int MPLSensor::setBatchDataRates() +{ + VFUNC_LOG; + + int res = 0; + int tempFd = -1; + + int64_t gyroRate; + int64_t accelRate; + int64_t compassRate; + int64_t pressureRate; + int64_t quatRate; + + int mplGyroRate; + int mplAccelRate; + int mplCompassRate; + int mplQuatRate; + +#ifdef ENABLE_MULTI_RATE + gyroRate = mBatchDelays[Gyro]; + /* take care of case where only one type of gyro sensors or + compass sensors is turned on */ + if (mBatchEnabled & (1 << Gyro) || mBatchEnabled & (1 << RawGyro)) { + gyroRate = (mBatchDelays[Gyro] <= mBatchDelays[RawGyro]) ? + (mBatchEnabled & (1 << Gyro) ? mBatchDelays[Gyro] : mBatchDelays[RawGyro]): + (mBatchEnabled & (1 << RawGyro) ? mBatchDelays[RawGyro] : mBatchDelays[Gyro]); + } + compassRate = mBatchDelays[MagneticField]; + if (mBatchEnabled & (1 << MagneticField) || mBatchEnabled & (1 << RawMagneticField)) { + compassRate = (mBatchDelays[MagneticField] <= mBatchDelays[RawMagneticField]) ? + (mBatchEnabled & (1 << MagneticField) ? mBatchDelays[MagneticField] : + mBatchDelays[RawMagneticField]) : + (mBatchEnabled & (1 << RawMagneticField) ? mBatchDelays[RawMagneticField] : + mBatchDelays[MagneticField]); + } + accelRate = mBatchDelays[Accelerometer]; + pressureRate = mBatchDelays[Pressure]; + + if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) || + (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) { + quatRate = mBatchDelays[GameRotationVector]; + mplQuatRate = (int) quatRate / 1000LL; + inv_set_quat_sample_rate(mplQuatRate); + inv_set_rotation_vector_6_axis_sample_rate(mplQuatRate); + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL rv 6 axis sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate, + 1000000000.f / quatRate ); + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL quat sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate, + 1000000000.f / quatRate ); + getDmpRate(&quatRate); + } + + mplGyroRate = (int) gyroRate / 1000LL; + mplAccelRate = (int) accelRate / 1000LL; + mplCompassRate = (int) compassRate / 1000LL; + + /* set rate in MPL */ + /* compass can only do 100Hz max */ + inv_set_gyro_sample_rate(mplGyroRate); + inv_set_accel_sample_rate(mplAccelRate); + inv_set_compass_sample_rate(mplCompassRate); + + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate); + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate); + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate); + +#else + /* search the minimum delay requested across all enabled sensors */ + int64_t wanted = 1000000000LL; + for (int i = 0; i < NumSensors; i++) { + if (mBatchEnabled & (1 << i)) { + int64_t ns = mBatchDelays[i]; + wanted = wanted < ns ? wanted : ns; + } + } + gyroRate = wanted; + accelRate = wanted; + compassRate = wanted; + pressureRate = wanted; +#endif + + /* takes care of gyro rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / gyroRate, mpu.gyro_rate, + getTimestamp()); + tempFd = open(mpu.gyro_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate); + if(res < 0) { + LOGE("HAL:GYRO update delay error"); + } + + /* takes care of accel rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / accelRate, mpu.accel_rate, + getTimestamp()); + tempFd = open(mpu.accel_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / accelRate); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + + /* takes care of compass rate */ + if (compassRate < mCompassSensor->getMinDelay() * 1000LL) { + compassRate = mCompassSensor->getMinDelay() * 1000LL; + } + mCompassSensor->setDelay(ID_M, compassRate); + + /* takes care of pressure rate */ + mPressureSensor->setDelay(ID_PS, pressureRate); + + return res; +} + +/* Set sensor rate */ +/* this function should be optimized */ +int MPLSensor::resetDataRates() +{ + VFUNC_LOG; + + int res = 0; + int tempFd = -1; + int64_t wanted = 1000000000LL; + + int64_t resetRate; + int64_t gyroRate; + int64_t accelRate; + int64_t compassRate; + int64_t pressureRate; + + if (!mEnabled) { + LOGV_IF(ENG_VERBOSE, "skip resetDataRates"); + return 0; + } + LOGI("HAL:resetDataRates mEnabled=%d", mEnabled); + /* search the minimum delay requested across all enabled sensors */ + /* skip setting rates if it is not changed */ + for (int i = 0; i < NumSensors; i++) { + if (mEnabled & (1 << i)) { + int64_t ns = mDelays[i]; + if ((wanted == ns) && (i != Pressure)) { + LOGV_IF(ENG_VERBOSE, "skip resetDataRates : same delay mDelays[%d]=%lld", i,mDelays[i]); + //return 0; + } + LOGV_IF(ENG_VERBOSE, "resetDataRates - mDelays[%d]=%lld", i, mDelays[i]); + wanted = wanted < ns ? wanted : ns; + } + } + + resetRate = wanted; + gyroRate = wanted; + accelRate = wanted; + compassRate = wanted; + pressureRate = wanted; + + /* set mpl data rate */ + inv_set_gyro_sample_rate((int)gyroRate/1000LL); + inv_set_accel_sample_rate((int)accelRate/1000LL); + inv_set_compass_sample_rate((int)compassRate/1000LL); + inv_set_linear_acceleration_sample_rate((int)resetRate/1000LL); + inv_set_orientation_sample_rate((int)resetRate/1000LL); + inv_set_rotation_vector_sample_rate((int)resetRate/1000LL); + inv_set_gravity_sample_rate((int)resetRate/1000LL); + inv_set_orientation_geomagnetic_sample_rate((int)resetRate/1000LL); + inv_set_rotation_vector_6_axis_sample_rate((int)resetRate/1000LL); + inv_set_geomagnetic_rotation_vector_sample_rate((int)resetRate/1000LL); + + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz", + gyroRate/1000LL, 1000000000.f / gyroRate); + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz", + accelRate/1000LL, 1000000000.f / accelRate); + LOGV_IF(PROCESS_VERBOSE, + "HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz", + compassRate/1000LL, 1000000000.f / compassRate); + + /* reset dmp rate */ + getDmpRate (&wanted); + + 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); + res = write_attribute_sensor(tempFd, 1000000000.f / wanted); + LOGE_IF(res < 0, "HAL:sampling frequency update delay error"); + + /* takes care of gyro rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / gyroRate, mpu.gyro_rate, + getTimestamp()); + tempFd = open(mpu.gyro_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate); + if(res < 0) { + LOGE("HAL:GYRO update delay error"); + } + + /* takes care of accel rate */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / accelRate, mpu.accel_rate, + getTimestamp()); + tempFd = open(mpu.accel_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / accelRate); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + + /* takes care of compass rate */ + if (compassRate < mCompassSensor->getMinDelay() * 1000LL) { + compassRate = mCompassSensor->getMinDelay() * 1000LL; + } + mCompassSensor->setDelay(ID_M, compassRate); + + /* takes care of pressure rate */ + mPressureSensor->setDelay(ID_PS, pressureRate); + + /* takes care of lpq case for data rate at 200Hz */ + if (checkLPQuaternion()) { + if (resetRate <= RATE_200HZ) { +#ifndef USE_LPQ_AT_FASTEST + enableLPQuaternion(0); +#endif + } + } + + return res; +} + +void MPLSensor::resetMplStates() +{ + VFUNC_LOG; + LOGV_IF(ENG_VERBOSE, "HAL:resetMplStates()"); + + inv_gyro_was_turned_off(); + inv_accel_was_turned_off(); + inv_compass_was_turned_off(); + inv_quaternion_sensor_was_turned_off(); + + return; +} + +void MPLSensor::initBias() +{ + VFUNC_LOG; + + LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0"); + if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to accel_x_dmp_bias"); + } + if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to accel_y_dmp_bias"); + } + if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to accel_z_dmp_bias"); + } + + if(write_attribute_sensor_continuous(accel_x_offset_fd, 0) < 0) { + LOGE("HAL:Error writing to accel_x_offset"); + } + if(write_attribute_sensor_continuous(accel_y_offset_fd, 0) < 0) { + LOGE("HAL:Error writing to accel_y_offset"); + } + if(write_attribute_sensor_continuous(accel_z_offset_fd, 0) < 0) { + LOGE("HAL:Error writing to accel_z_offset"); + } + + if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_x_dmp_bias"); + } + if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_y_dmp_bias"); + } + if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_z_dmp_bias"); + } + + if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_x_offset"); + } + if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_y_offset"); + } + if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0) { + LOGE("HAL:Error writing to gyro_z_offset"); + } + return; +} + +/*TODO: reg_dump in a separate file*/ +void MPLSensor::sys_dump(bool fileMode) +{ + VFUNC_LOG; + + char sysfs_path[MAX_SYSFS_NAME_LEN]; + char scan_element_path[MAX_SYSFS_NAME_LEN]; + + memset(sysfs_path, 0, sizeof(sysfs_path)); + memset(scan_element_path, 0, sizeof(scan_element_path)); + inv_get_sysfs_path(sysfs_path); + sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements"); + + read_sysfs_dir(fileMode, sysfs_path); + read_sysfs_dir(fileMode, scan_element_path); + + dump_dmp_img("/data/local/read_img.h"); + return; +} diff --git a/6515/libsensors_iio/MPLSensor.h b/6515/libsensors_iio/MPLSensor.h new file mode 100644 index 0000000..65e3b23 --- /dev/null +++ b/6515/libsensors_iio/MPLSensor.h @@ -0,0 +1,557 @@ +/* +* 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 <time.h> +#include <utils/Vector.h> +#include <utils/KeyedVector.h> +#include <utils/String8.h> +#include "sensors.h" +#include "SensorBase.h" +#include "InputEventReader.h" + +#ifndef INVENSENSE_COMPASS_CAL +#pragma message("unified HAL for AKM") +#include "CompassSensor.AKM.h" +#endif + +#ifdef SENSOR_ON_PRIMARY_BUS +#pragma message("Sensor on Primary Bus") +#include "CompassSensor.IIO.primary.h" +#else +#pragma message("Sensor on Secondary Bus") +#include "CompassSensor.IIO.9150.h" +#endif + +class PressureSensor; + +/*****************************************************************************/ +/* 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_ONE_AXIS_PRESSURE (0x0400) +#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 + +// mask of virtual sensors that require gyro + accel + compass data +#define VIRTUAL_SENSOR_9AXES_MASK ( \ + (1 << Orientation) \ + | (1 << RotationVector) \ + | (1 << LinearAccel) \ + | (1 << Gravity) \ +) +// mask of virtual sensors that require gyro + accel data (but no compass data) +#define VIRTUAL_SENSOR_GYRO_6AXES_MASK ( \ + (1 << GameRotationVector) \ +) +// mask of virtual sensors that require mag + accel data (but no gyro data) +#define VIRTUAL_SENSOR_MAG_6AXES_MASK ( \ + (1 << GeomagneticRotationVector) \ +) +// mask of all virtual sensors +#define VIRTUAL_SENSOR_ALL_MASK ( \ + VIRTUAL_SENSOR_9AXES_MASK \ + | VIRTUAL_SENSOR_GYRO_6AXES_MASK \ + | VIRTUAL_SENSOR_MAG_6AXES_MASK \ +) + +// bit mask of current MPL active features (mMplFeatureActiveMask) +#define INV_COMPASS_CAL 0x01 +#define INV_COMPASS_FIT 0x02 + +// bit mask of current DMP active features (mFeatureActiveMask) +#define INV_DMP_QUATERNION 0x001 //3 elements without real part, 32 bit each +#define INV_DMP_DISPL_ORIENTATION 0x002 //screen orientation +#define INV_DMP_SIGNIFICANT_MOTION 0x004 //significant motion +#define INV_DMP_PEDOMETER 0x008 //interrupt-based pedometer +#define INV_DMP_PEDOMETER_STEP 0x010 //timer-based pedometer +#define INV_DMP_PED_STANDALONE 0x020 //timestamps only +#define INV_DMP_6AXIS_QUATERNION 0x040 //3 elements without real part, 32 bit each +#define INV_DMP_PED_QUATERNION 0x080 //3 elements without real part, 16 bit each +#define INV_DMP_PED_INDICATOR 0x100 //tag along header with step indciator +#define INV_DMP_BATCH_MODE 0x200 //batch mode + +// bit mask of whether DMP should be turned on +#define DMP_FEATURE_MASK ( \ + (INV_DMP_QUATERNION) \ + | (INV_DMP_DISPL_ORIENTATION) \ + | (INV_DMP_SIGNIFICANT_MOTION) \ + | (INV_DMP_PEDOMETER) \ + | (INV_DMP_PEDOMETER_STEP) \ + | (INV_DMP_6AXIS_QUATERNION) \ + | (INV_DMP_PED_QUATERNION) \ + | (INV_DMP_BATCH_MODE) \ +) + +// bit mask of DMP features as sensors +#define DMP_SENSOR_MASK ( \ + (INV_DMP_DISPL_ORIENTATION) \ + | (INV_DMP_SIGNIFICANT_MOTION) \ + | (INV_DMP_PEDOMETER) \ + | (INV_DMP_PEDOMETER_STEP) \ + | (INV_DMP_6AXIS_QUATERNION) \ +) + +// data header format used by kernel driver. +#define DATA_FORMAT_STEP 0x0001 +#define DATA_FORMAT_MARKER 0x0010 +#define DATA_FORMAT_EMPTY_MARKER 0x0020 +#define DATA_FORMAT_PED_STANDALONE 0x0100 +#define DATA_FORMAT_PED_QUAT 0x0200 +#define DATA_FORMAT_6_AXIS 0x0400 +#define DATA_FORMAT_QUAT 0x0800 +#define DATA_FORMAT_COMPASS 0x1000 +#define DATA_FORMAT_GYRO 0x2000 +#define DATA_FORMAT_ACCEL 0x4000 +#define DATA_FORMAT_PRESSURE 0x8000 +#define DATA_FORMAT_MASK 0xffff + +#define BYTES_PER_SENSOR 8 +#define BYTES_PER_SENSOR_PACKET 16 +#define QUAT_ONLY_LAST_PACKET_OFFSET 16 +#define BYTES_QUAT_DATA 24 +#define MAX_READ_SIZE BYTES_QUAT_DATA +#define MAX_SUSPEND_BATCH_PACKET_SIZE 1024 +#define MAX_PACKET_SIZE 80 //8 * 4 + (2 * 24) + +/* Uncomment to enable Low Power Quaternion */ +#define ENABLE_LP_QUAT_FEAT + +/* Enable Pressure sensor support */ +#define ENABLE_PRESSURE + +/* Screen Orientation is not currently supported */ +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: + + 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); + virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout); + virtual int flush(int handle); + int selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask); + int checkBatchEnabled(); + int setBatch(int en, int toggleEnable); + int32_t getEnableMask() { return mEnabled; } + void getHandle(int32_t handle, int &what, android::String8 &sname); + + 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 int getStepCountPollTime(); + virtual bool hasPendingEvents() const; + virtual bool hasStepCountPendingEvents(); + int populateSensorList(struct sensor_t *list, int len); + + int readAccelEvents(sensors_event_t* data, int count); + void buildCompassEvent(); + void buildMpuEvent(); + int checkValidHeader(unsigned short data_format); + + int turnOffAccelFifo(); + int turnOffGyroFifo(); + 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(); + + int getDmpSignificantMotionFd(); + int readDmpSignificantMotionEvents(sensors_event_t* data, int count); + int enableDmpSignificantMotion(int); + int significantMotionHandler(sensors_event_t* data); + bool checkSmdSupport(){return (mDmpSignificantMotionEnabled);}; + + int enableDmpPedometer(int, int); + int readDmpPedometerEvents(sensors_event_t* data, int count, int32_t id, int outputType); + int getDmpPedometerFd(); + bool checkPedometerSupport() {return (mDmpPedometerEnabled || mDmpStepCountEnabled);}; + bool checkOrientationSupport() {return ((isDmpDisplayOrientationOn() + && (mDmpOrientationEnabled + || !isDmpScreenAutoRotationEnabled())));}; + +protected: + CompassSensor *mCompassSensor; + PressureSensor *mPressureSensor; + + 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 rawCompassHandler(sensors_event_t *data); + int rvHandler(sensors_event_t *data); + int grvHandler(sensors_event_t *data); + int laHandler(sensors_event_t *data); + int gravHandler(sensors_event_t *data); + int orienHandler(sensors_event_t *data); + int smHandler(sensors_event_t *data); + int pHandler(sensors_event_t *data); + int gmHandler(sensors_event_t *data); + int psHandler(sensors_event_t *data); + int sdHandler(sensors_event_t *data); + int scHandler(sensors_event_t *data); + int metaHandler(sensors_event_t *data, int flags); + 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 setAccelInitialState(); + int masterEnable(int en); + int enablePedStandalone(int en); + int enablePedStandaloneData(int en); + int enablePedQuaternion(int); + int enablePedQuaternionData(int); + int enable6AxisQuaternion(int); + int enable6AxisQuaternionData(int); + int enableLPQuaternion(int); + int enableQuaternionData(int); + int enableAccelPedometer(int); + int enableAccelPedData(int); + int onDmp(int); + int enableGyro(int en); + int enableLowPowerAccel(int en); + int enableAccel(int en); + int enableCompass(int en, int rawSensorOn); + int enablePressure(int en); + int enableBatch(int64_t timeout); + void computeLocalSensorMask(int enabled_sensors); + int computeBatchSensorMask(int enableSensor, int checkNewBatchSensor); + int computeBatchDataOutput(); + int enableSensors(unsigned long sensors, int en, uint32_t changed); + 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); + int resetCompass(void); + void setCompassDelay(int64_t ns); + void enable_iio_sysfs(void); + int setDmpFeature(int en); + int computeAndSetDmpState(void); + int enablePedometer(int); + int enablePedIndicator(int en); + int checkPedStandaloneEnabled(void); + int checkPedQuatEnabled(); + int check6AxisQuatEnabled(); + int checkLPQRateSupported(); + int checkLPQuaternion(); + int checkAccelPed(); + void setInitial6QuatValue(); + int writeSignificantMotionParams(bool toggleEnable, + uint32_t delayThreshold1, uint32_t delayThreshold2, + uint32_t motionThreshold); + long mMasterSensorMask; + long mLocalSensorMask; + int mPollTime; + int64_t mStepCountPollTime; + 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]; + 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 accel_x_offset_fd; + int accel_y_offset_fd; + int accel_z_offset_fd; + + int accel_x_dmp_bias_fd; + int accel_y_dmp_bias_fd; + int accel_z_dmp_bias_fd; + + int gyro_x_offset_fd; + int gyro_y_offset_fd; + int gyro_z_offset_fd; + + int gyro_x_dmp_bias_fd; + int gyro_y_dmp_bias_fd; + int gyro_z_dmp_bias_fd; + + int dmp_orient_fd; + int mDmpOrientationEnabled; + + int dmp_sign_motion_fd; + int mDmpSignificantMotionEnabled; + + int dmp_pedometer_fd; + int mDmpPedometerEnabled; + int mDmpStepCountEnabled; + + uint32_t mEnabled; + uint32_t mBatchEnabled; + int32_t mFlushSensorEnabled; + uint32_t mOldBatchEnabledMask; + int64_t mBatchTimeoutInMs; + sensors_event_t mPendingEvents[NumSensors]; + sensors_event_t mSmEvents; + sensors_event_t mSdEvents; + sensors_event_t mScEvents; + int64_t mDelays[NumSensors]; + int64_t mBatchDelays[NumSensors]; + int64_t mBatchTimeouts[NumSensors]; + hfunc_t mHandlers[NumSensors]; + short mCachedGyroData[3]; + long mCachedAccelData[3]; + long mCachedCompassData[3]; + long mCachedQuaternionData[3]; + long mCached6AxisQuaternionData[3]; + long mCachedPedQuaternionData[3]; + long mCachedPressureData; + android::KeyedVector<int, int> mIrqFds; + + InputEventCircularReader mAccelInputReader; + InputEventCircularReader mGyroInputReader; + + bool mFirstRead; + short mTempScale; + short mTempOffset; + int64_t mTempCurrentTime; + int mAccelScale; + long mAccelSelfTestScale; + long mGyroScale; + long mGyroSelfTestScale; + long mCompassScale; + float mCompassBias[3]; + bool mFactoryGyroBiasAvailable; + long mFactoryGyroBias[3]; + bool mGyroBiasAvailable; + bool mGyroBiasApplied; + float mGyroBias[3]; //in body frame + long mGyroChipBias[3]; //in chip frame + bool mFactoryAccelBiasAvailable; + long mFactoryAccelBias[3]; + bool mAccelBiasAvailable; + bool mAccelBiasApplied; + long mAccelBias[3]; //in chip frame + + uint32_t mPendingMask; + unsigned long mSensorMask; + + char chip_ID[MAX_CHIP_ID_LEN]; + char mSysfsPath[MAX_SYSFS_NAME_LEN]; + + signed char mGyroOrientation[9]; + signed char mAccelOrientation[9]; + + int64_t mSensorTimestamp; + int64_t mCompassTimestamp; + int64_t mPressureTimestamp; + + struct sysfs_attrbs { + char *chip_enable; + char *power_state; + char *master_enable; + char *dmp_firmware; + char *firmware_loaded; + char *dmp_on; + char *dmp_int_on; + char *dmp_event_int_on; + char *tap_on; + char *key; + char *self_test; + char *temperature; + + char *gyro_enable; + char *gyro_fifo_rate; + char *gyro_fsr; + char *gyro_orient; + char *gyro_fifo_enable; + char *gyro_rate; + + char *accel_enable; + char *accel_fifo_rate; + char *accel_fsr; + char *accel_bias; + char *accel_orient; + char *accel_fifo_enable; + char *accel_rate; + + char *three_axis_q_on; //formerly quaternion_on + char *three_axis_q_rate; + + char *six_axis_q_on; + char *six_axis_q_rate; + + char *six_axis_q_value; + + char *ped_q_on; + char *ped_q_rate; + + char *step_detector_on; + char *step_indicator_on; + + char *in_timestamp_en; + char *in_timestamp_index; + char *in_timestamp_type; + + char *buffer_length; + + char *display_orientation_on; + char *event_display_orientation; + + char *in_accel_x_offset; + char *in_accel_y_offset; + char *in_accel_z_offset; + char *in_accel_self_test_scale; + + char *in_accel_x_dmp_bias; + char *in_accel_y_dmp_bias; + char *in_accel_z_dmp_bias; + + char *in_gyro_x_offset; + char *in_gyro_y_offset; + char *in_gyro_z_offset; + char *in_gyro_self_test_scale; + + char *in_gyro_x_dmp_bias; + char *in_gyro_y_dmp_bias; + char *in_gyro_z_dmp_bias; + + char *event_smd; + char *smd_enable; + char *smd_delay_threshold; + char *smd_delay_threshold2; + char *smd_threshold; + char *batchmode_timeout; + char *batchmode_wake_fifo_full_on; + char *flush_batch; + + char *pedometer_on; + char *pedometer_int_on; + char *event_pedometer; + char *pedometer_steps; + char *pedometer_counter; + + char *motion_lpa_on; + } mpu; + + char *sysfs_names_ptr; + int mMplFeatureActiveMask; + uint64_t mFeatureActiveMask; + bool mDmpOn; + int mPedUpdate; + int mPressureUpdate; + int64_t mQuatSensorTimestamp; + int64_t mStepSensorTimestamp; + uint64_t mLastStepCount; + int mLeftOverBufferSize; + char mLeftOverBuffer[1024]; + bool mInitial6QuatValueAvailable; + long mInitial6QuatValue[4]; + bool mFlushBatchSet; + uint32_t mSkipReadEvents; + bool mDataMarkerDetected; + bool mEmptyDataMarkerDetected; + +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 fillGMRV(struct sensor_t *list); + void fillGRV(struct sensor_t *list); + void fillOrientation(struct sensor_t *list); + void fillGravity(struct sensor_t *list); + void fillLinearAccel(struct sensor_t *list); + void fillSignificantMotion(struct sensor_t *list); +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + void fillScreenOrientation(struct sensor_t *list); +#endif + void storeCalibration(); + void loadDMP(); + bool isMpuNonDmp(); + int isLowPowerQuatEnabled(); + int isDmpDisplayOrientationOn(); + void getCompassBias(); + void getFactoryGyroBias(); + void setFactoryGyroBias(); + void getGyroBias(); + void setGyroZeroBias(); + void setGyroBias(); + void getFactoryAccelBias(); + void setFactoryAccelBias(); + void getAccelBias(); + void setAccelBias(); + int isCompassDisabled(); + int setBatchDataRates(); + int resetDataRates(); + void initBias(); + void resetMplStates(); + void sys_dump(bool fileMode); +}; + +extern "C" { + void setCallbackObject(MPLSensor*); + MPLSensor *getCallbackObject(); +} + +#endif // ANDROID_MPL_SENSOR_H diff --git a/6515/libsensors_iio/MPLSensorSysApi.cpp b/6515/libsensors_iio/MPLSensorSysApi.cpp new file mode 100644 index 0000000..92e8ea1 --- /dev/null +++ b/6515/libsensors_iio/MPLSensorSysApi.cpp @@ -0,0 +1,169 @@ +/* +* 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. +*/ + +#undef NDEBUG +#define NDEBUG 0 + +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <float.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <stdlib.h> +#include <sys/select.h> +#include <dlfcn.h> +#include <pthread.h> + +#include <cutils/log.h> +#include <utils/KeyedVector.h> +#include <linux/input.h> + +#include "invensense.h" + +#include "MPLSensorSysApi.h" + +/*******************************************************************************/ +/* Gyro Driver Specific SYSFS Attribute */ +/*******************************************************************************/ +#define GYRO_SENSOR_SYSFS_PATH "/sys/class/input/input0/" +#define GYRO_SENSOR_SELFTEST "device/inv_gyro/self_test" + +MplSys_Interface* getSysInterfaceObject() +{ + MPLSensorSysApi* s = static_cast<MPLSensorSysApi*>(MPLSensor::gMPLSensor); + return static_cast<MplSys_Interface*>(s); +} + +MPLSensorSysApi::MPLSensorSysApi() : MPLSensor() +{ + +} + +MPLSensorSysApi::~MPLSensorSysApi() +{ + +} + +MPLSensorSysApi::MplSys_Interface::~MplSys_Interface() +{ + +} + +/* Should be getting from hardware self-test */ +int MPLSensorSysApi::getBiases(float *b) +{ + FUNC_LOG; + int rv = INV_SUCCESS; + long val[3]; + long temp; + LOGV("get biases\n"); + pthread_mutex_lock(&mMplMutex); + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +int MPLSensorSysApi::setBiases(float *b) +{ + FUNC_LOG; + int rv = INV_SUCCESS; + pthread_mutex_lock(&mMplMutex); + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +int MPLSensorSysApi::setBiasUpdateFunc(long f) +{ + FUNC_LOG; + LOGW("SysApi :: setBiasUpdateFunc is OBSOLETE and ineffective"); + return 0; +} + +int MPLSensorSysApi::setSensors(long s) +{ + FUNC_LOG; + int rv = INV_SUCCESS; + + pthread_mutex_lock(&mMplMutex); + mMasterSensorMask = s; + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +int MPLSensorSysApi::getSensors(long* s) +{ + FUNC_LOG; + int rv = INV_SUCCESS; + pthread_mutex_lock(&mMplMutex); + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +int MPLSensorSysApi::resetCal() +{ + FUNC_LOG; + int rv = INV_SUCCESS; + LOGI("SysApi :: resetCal is OBSOLETE"); + return rv; +} + +int MPLSensorSysApi::selfTest() +{ + FUNC_LOG; + int rv = INV_SUCCESS; + char buf[50]; + + LOGV_IF(EXTRA_VERBOSE, "gyro set delay path: %s", GYRO_SENSOR_SELF_TEST); + + int fd = open(mpu.self_test, O_RDONLY); + if( fd < 0 ) { + LOGE("Error opening gyro self-test"); + return INV_ERROR; + } + pthread_mutex_lock(&mMplMutex); + char x[15], y[15], z[15]; + char result[2]; + do { + memset(buf, 0, sizeof(buf)); + int count = read_attribute_sensor(fd, buf, sizeof(buf)); + if( count < 1 ) { + LOGE("Error reading gyro self-test"); + pthread_mutex_unlock(&mMplMutex); + return INV_ERROR; + } + sscanf(buf, "%[^','],%[^','],%[^','],%[^',']", x, y, z, result); + LOGI("Bias: X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); + if ( atoi(result) ) { + LOGE("self test passed"); + } + else { + LOGE("error self-test failed"); + break; + } + } while (0); + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +int MPLSensorSysApi::setLocalMagField(float x, float y, float z) +{ + FUNC_LOG; + int rv = INV_SUCCESS; + LOGI("SysApi :: setLocalMagField is OBSOLETE"); + return rv; +} + diff --git a/6515/libsensors_iio/MPLSensorSysApi.h b/6515/libsensors_iio/MPLSensorSysApi.h new file mode 100644 index 0000000..3c1c53e --- /dev/null +++ b/6515/libsensors_iio/MPLSensorSysApi.h @@ -0,0 +1,49 @@ +/* +* 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_HAL_SYS_API_H +#define INV_HAL_SYS_API_H + +#include "sensors.h" +#include "MPLSensor.h" +#include <gui/MplInterfaces.h> + +class MPLSensorSysApi : public MPLSensor, public MplSys_Interface { + +public: + MPLSensorSysApi(); + virtual ~MPLSensorSysApi(); + + virtual int getBiases(float *b); + virtual int setBiases(float *b); + virtual int setBiasUpdateFunc(long f); + virtual int setSensors(long s); + virtual int getSensors(long* s); + virtual int resetCal(); + virtual int selfTest(); + virtual int setLocalMagField(float x, float y, float z); + +private: + int write_attribute_sensor(int fd, unsigned char* buf, size_t count); + +}; + +extern "C" { +MplSys_Interface* getSysInterfaceObject(); +} + +#endif + diff --git a/6515/libsensors_iio/MPLSupport.cpp b/6515/libsensors_iio/MPLSupport.cpp new file mode 100644 index 0000000..08b48bb --- /dev/null +++ b/6515/libsensors_iio/MPLSupport.cpp @@ -0,0 +1,387 @@ +/* +* 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 <dirent.h> +#include <string.h> +#include <stdio.h> +#include "log.h" +#include "SensorBase.h" +#include <fcntl.h> + +#include "ml_sysfs_helper.h" +#include "ml_load_dmp.h" + +int inv_read_data(char *fname, long *data) +{ + VFUNC_LOG; + + char buf[sizeof(long) * 4]; + int count, fd; + + fd = open(fname, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening %s", fname); + return -1; + } + memset(buf, 0, sizeof(buf)); + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + close(fd); + return -1; + } else { + count = sscanf(buf, "%ld", data); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data); + } + close(fd); + + return 0; +} + +/* This one DOES NOT close FDs for you */ +int read_attribute_sensor(int fd, char* data, unsigned int size) +{ + VFUNC_LOG; + + int count = 0; + if (fd > 0) { + count = pread(fd, data, size, 0); + if(count < 1) { + LOGE("HAL:read fails with error code=%d", count); + } + } + return count; +} + +/** + * @brief Enable a sensor through the sysfs file descriptor + * provided. + * @note this function one closes FD after the write + * @param fd + * the file descriptor to write into + * @param en + * the value to write, typically 1 or 0 + * @return the errno whenever applicable. + */ +int enable_sysfs_sensor(int fd, int en) +{ + VFUNC_LOG; + + int nb; + int err = 0; + + char c = en ? '1' : '0'; + nb = write(fd, &c, 1); + + if (nb <= 0) { + err = errno; + LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)", + c, nb, strerror(err), err); + } + close(fd); + + + return -err; +} + +/* This one closes FDs for you */ +int write_attribute_sensor(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); + } + close(fd); + } + + 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; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "r"); + if (sysfsfp != NULL) { + if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) { + res = errno; + LOGE("HAL:ERR open file %s to read with error %d", filename, res); + } + } + return -res; +} + +int read_sysfs_int64(char *filename, int64_t *var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "r"); + if (sysfsfp != NULL) { + if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) { + res = errno; + LOGE("HAL:ERR open file %s to read with error %d", filename, res); + } + } + return -res; +} + +void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement) +{ + int bytePosition = 0; + for (int index = 0; index < numElement; index++) { + for (int i = 0; i < 4; i++) { + hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF); + //LOGI("e%d quat[%d]: %x", index, bytePosition, hex[bytePosition]); + bytePosition++; + } + } + return; +} + +int write_sysfs_int(char *filename, int var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "w"); + if (sysfsfp != NULL) { + if (fprintf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) { + res = errno; + LOGE("HAL:ERR open file %s to write with error %d", filename, res); + } + } + return -res; +} + +int write_sysfs_longlong(char *filename, int64_t var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "w"); + if (sysfsfp != NULL) { + if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) { + res = errno; + LOGE("HAL:ERR open file %s to write with error %d", filename, res); + } + } + 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; +} + +void dump_dmp_img(const char *outFile) +{ + FILE *fp; + int i; + + char sysfs_path[MAX_SYSFS_NAME_LEN]; + char dmp_path[MAX_SYSFS_NAME_LEN]; + + inv_get_sysfs_path(sysfs_path); + sprintf(dmp_path, "%s%s", sysfs_path, "/dmp_firmware"); + + LOGI("HAL DEBUG:dump DMP image"); + LOGI("HAL DEBUG:open %s\n", dmp_path); + LOGI("HAL DEBUG:write to %s", outFile); + + read_dmp_img(dmp_path, (char *)outFile); +} + +int read_sysfs_dir(bool fileMode, char *sysfs_path) +{ + VFUNC_LOG; + + int res = 0; + char full_path[MAX_SYSFS_NAME_LEN]; + int fd; + char buf[sizeof(long) *4]; + long data; + + DIR *dp; + struct dirent *ep; + + dp = opendir (sysfs_path); + + if (dp != NULL) + { + LOGI("******************** System Sysfs Dump ***************************"); + LOGV_IF(0,"HAL DEBUG: opened directory %s", sysfs_path); + while ((ep = readdir (dp))) { + if(ep != NULL) { + LOGV_IF(0,"file name %s", ep->d_name); + if(!strcmp(ep->d_name, ".") || !strcmp(ep->d_name, "..") || + !strcmp(ep->d_name, "uevent") || !strcmp(ep->d_name, "dev") || + !strcmp(ep->d_name, "self_test")) + continue; + sprintf(full_path, "%s%s%s", sysfs_path, "/", ep->d_name); + LOGV_IF(0,"HAL DEBUG: reading %s", full_path); + fd = open(full_path, O_RDONLY); + if (fd > -1) { + memset(buf, 0, sizeof(buf)); + res = read_attribute_sensor(fd, buf, sizeof(buf)); + close(fd); + if (res > 0) { + res = sscanf(buf, "%ld", &data); + if (res) + LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data); + } else { + LOGV_IF(0,"HAL DEBUG: error reading %s", full_path); + } + } else { + LOGV_IF(0,"HAL DEBUG: error opening %s", full_path); + } + close(fd); + } + } + closedir(dp); + } else{ + LOGI("HAL DEBUG: could not open directory %s", sysfs_path); + } + + return res; +} + +int inv_float_to_q16(float *fdata, long *ldata) +{ + + if (!fdata || !ldata) + return -1; + ldata[0] = (long)(fdata[0] * 65536.f); + ldata[1] = (long)(fdata[1] * 65536.f); + ldata[2] = (long)(fdata[2] * 65536.f); + return 0; +} + +int inv_long_to_q16(long *fdata, long *ldata) +{ + + if (!fdata || !ldata) + return -1; + ldata[0] = (fdata[1] * 65536.f); + ldata[1] = (fdata[2] * 65536.f); + ldata[2] = (fdata[3] * 65536.f); + return 0; +} + +int inv_float_to_round(float *fdata, long *ldata) +{ + + if (!fdata || !ldata) + return -1; + ldata[0] = (long)fdata[0]; + ldata[1] = (long)fdata[1]; + ldata[2] = (long)fdata[2]; + return 0; +} + +int inv_float_to_round2(float *fdata, short *ldata) +{ + + if (!fdata || !ldata) + return -1; + ldata[0] = (short)fdata[0]; + ldata[1] = (short)fdata[1]; + ldata[2] = (short)fdata[2]; + return 0; +} + +int inv_long_to_float(long *ldata, float *fdata) +{ + + if (!ldata || !fdata) + return -1; + fdata[0] = (float)ldata[0]; + fdata[1] = (float)ldata[1]; + fdata[2] = (float)ldata[2]; + return 0; +} diff --git a/6515/libsensors_iio/MPLSupport.h b/6515/libsensors_iio/MPLSupport.h new file mode 100644 index 0000000..c7f8eff --- /dev/null +++ b/6515/libsensors_iio/MPLSupport.h @@ -0,0 +1,43 @@ +/* +* 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_SUPPORT_H +#define ANDROID_MPL_SUPPORT_H + +#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_int64(char*, int64_t*); +int read_sysfs_int(char*, int*); +int write_sysfs_int(char*, int); +int write_sysfs_longlong(char*, long long); +int fill_dev_full_name_by_prefix(const char* dev_prefix, + char* dev_full_name, int len); +void dump_dmp_img(const char *out_file); +int read_sysfs_dir(bool fileMode, char *sysfs_path); + +void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement); +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_long_to_float(long *ldata, float *fdata); + +#endif // ANDROID_MPL_SUPPORT_H diff --git a/6515/libsensors_iio/PressureSensor.IIO.secondary.cpp b/6515/libsensors_iio/PressureSensor.IIO.secondary.cpp new file mode 100644 index 0000000..1a33c19 --- /dev/null +++ b/6515/libsensors_iio/PressureSensor.IIO.secondary.cpp @@ -0,0 +1,200 @@ +/* + * 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 "PressureSensor.IIO.secondary.h" +#include "sensors.h" +#include "MPLSupport.h" +#include "sensor_params.h" +#include "ml_sysfs_helper.h" + +#pragma message("HAL:build pressure sensor on Invensense MPU secondary bus") +/* dynamically get this when driver supports it */ +#define CHIP_ID "BMP280" + +//#define TIMER (1) +#define DEFAULT_POLL_TIME 300 +#define PRESSURE_MAX_SYSFS_ATTRB sizeof(pressureSysFs) / sizeof(char*) + +static int s_poll_time = -1; +static int min_poll_time = 50; +static struct timespec t_pre; + +/*****************************************************************************/ + +PressureSensor::PressureSensor(const char *sysfs_path) + : SensorBase(NULL, NULL), + pressure_fd(-1) +{ + VFUNC_LOG; + + mSysfsPath = sysfs_path; + LOGV_IF(ENG_VERBOSE, "pressuresensor path: %s", mSysfsPath); + if(inv_init_sysfs_attributes()) { + LOGE("Error Instantiating Pressure Sensor\n"); + return; + } else { + LOGI_IF(PROCESS_VERBOSE, "HAL:Secondary Chip Id: %s", CHIP_ID); + } +} + +PressureSensor::~PressureSensor() +{ + VFUNC_LOG; + + if( pressure_fd > 0) + close(pressure_fd); +} + +int PressureSensor::getFd() const +{ + VHANDLER_LOG; + return pressure_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 PressureSensor::enable(int32_t handle, int en) +{ + VFUNC_LOG; + + int res = 0; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, pressureSysFs.pressure_enable, getTimestamp()); + res = write_sysfs_int(pressureSysFs.pressure_enable, en); + + return res; +} + +int PressureSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + + int res = 0; + + mDelay = int(1000000000.f / ns); + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)", + mDelay, pressureSysFs.pressure_rate, getTimestamp()); + res = write_sysfs_int(pressureSysFs.pressure_rate, mDelay); + +#ifdef TIMER + int t_poll_time = (int)(ns / 1000000LL); + if (t_poll_time > min_poll_time) { + s_poll_time = t_poll_time; + } else { + s_poll_time = min_poll_time; + } + LOGV_IF(PROCESS_VERBOSE, + "HAL:setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f/ns); +#endif + return res; +} + + +/** + @brief This function will return the state of the sensor. + @return 1=enabled; 0=disabled +**/ +int PressureSensor::getEnable(int32_t handle) +{ + VFUNC_LOG; + return mEnable; +} + +/** + * @brief This function will return the current delay for this sensor. + * @return delay in nanoseconds. + */ +int64_t PressureSensor::getDelay(int32_t handle) +{ + VFUNC_LOG; + +#ifdef TIMER + if (mEnable) { + return s_poll_time; + } else { + return -1; + } +#endif + return mDelay; +} + +void PressureSensor::fillList(struct sensor_t *list) +{ + VFUNC_LOG; + + const char *pressure = "BMP280"; + + if (pressure) { + if(!strcmp(pressure, "BMP280")) { + list->maxRange = PRESSURE_BMP280_RANGE; + list->resolution = PRESSURE_BMP280_RESOLUTION; + list->power = PRESSURE_BMP280_POWER; + list->minDelay = PRESSURE_BMP280_MINDELAY; + mMinDelay = list->minDelay; + return; + } + } + LOGE("HAL:unknown pressure id %s -- " + "params default to bmp280 and might be wrong.", + pressure); + list->maxRange = PRESSURE_BMP280_RANGE; + list->resolution = PRESSURE_BMP280_RESOLUTION; + list->power = PRESSURE_BMP280_POWER; + list->minDelay = PRESSURE_BMP280_MINDELAY; + mMinDelay = list->minDelay; + return; +} + +int PressureSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + pathP = (char*)malloc(sizeof(char[PRESSURE_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + char *sptr = pathP; + char **dptr = (char**)&pressureSysFs; + if (sptr == NULL) + return -1; + unsigned char i = 0; + do { + *dptr++ = sptr; + memset(sptr, 0, sizeof(sptr)); + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < PRESSURE_MAX_SYSFS_ATTRB); + + sprintf(pressureSysFs.pressure_enable, "%s%s", mSysfsPath, "/pressure_enable"); + sprintf(pressureSysFs.pressure_rate, "%s%s", mSysfsPath, "/pressure_rate"); + return 0; +} diff --git a/6515/libsensors_iio/PressureSensor.IIO.secondary.h b/6515/libsensors_iio/PressureSensor.IIO.secondary.h new file mode 100644 index 0000000..3dae748 --- /dev/null +++ b/6515/libsensors_iio/PressureSensor.IIO.secondary.h @@ -0,0 +1,76 @@ +/* + * 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 PRESSURE_SENSOR_H +#define PRESSURE_SENSOR_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.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" + +class PressureSensor : public SensorBase { + +public: + PressureSensor(const char *sysfs_path); + virtual ~PressureSensor(); + + 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); + virtual int64_t getMinDelay() { return mMinDelay; } + // only applicable to primary + virtual int readEvents(sensors_event_t *data, int count) { return 0; } + + void fillList(struct sensor_t *list); + // default is integrated for secondary bus + int isIntegrated() { return (1); } + +private: + char sensor_name[200]; + + struct sysfs_attrbs { + char *pressure_enable; + char *pressure_rate; + } pressureSysFs; + + int pressure_fd; + int64_t mDelay; + int64_t mMinDelay; + int mEnable; + char* pathP; + const char* mSysfsPath; + + int inv_init_sysfs_attributes(void); +}; + +/*****************************************************************************/ + +#endif // PRESSURE_SENSOR_H diff --git a/6515/libsensors_iio/SensorBase.cpp b/6515/libsensors_iio/SensorBase.cpp new file mode 100644 index 0000000..016d2df --- /dev/null +++ b/6515/libsensors_iio/SensorBase.cpp @@ -0,0 +1,211 @@ +/* +* 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 <stdlib.h> +#include <dirent.h> +#include <sys/select.h> +#include <cutils/log.h> +#include <linux/input.h> + +#include <cutils/properties.h> + +#include "SensorBase.h" + +/*****************************************************************************/ + +// static vars +bool SensorBase::PROCESS_VERBOSE = false; +bool SensorBase::EXTRA_VERBOSE = false; +bool SensorBase::SYSFS_VERBOSE = false; + +bool SensorBase::FUNC_ENTRY = false; +bool SensorBase::HANDLER_ENTRY = false; +bool SensorBase::ENG_VERBOSE = false; +bool SensorBase::INPUT_DATA = false; +bool SensorBase::HANDLER_DATA = false; +bool SensorBase::DEBUG_BATCHING = false; + +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); + } + + char value[PROPERTY_VALUE_MAX]; + property_get("invn.hal.verbose.basic", value, "0"); + if (atoi(value)) { + PROCESS_VERBOSE = true; + } + property_get("invn.hal.verbose.extra", value, "0"); + if (atoi(value)) { + EXTRA_VERBOSE = true; + } + property_get("invn.hal.verbose.sysfs", value, "0"); + if (atoi(value)) { + SYSFS_VERBOSE = true; + } + property_get("invn.hal.verbose.engineering", value, "0"); + if (atoi(value)) { + ENG_VERBOSE = true; + } + property_get("invn.hal.entry.function", value, "0"); + if (atoi(value)) { + FUNC_ENTRY = true; + } + property_get("invn.hal.entry.handler", value, "0"); + if (atoi(value)) { + HANDLER_ENTRY = true; + } + property_get("invn.hal.data.input", value, "0"); + if (atoi(value)) { + INPUT_DATA = true; + } + property_get("invn.hal.data.handler", value, "0"); + if (atoi(value)) { + HANDLER_DATA = true; + } + property_get("invn.hal.debug.batching", value, "0"); + if (atoi(value)) { + DEBUG_BATCHING = true; + } +} + +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; +} + +int SensorBase::query(int what, int* value) +{ + return 0; +} + +int SensorBase::batch(int handle, int flags, int64_t period_ns, int64_t timeout) +{ + return 0; +} + +int SensorBase::flush(int handle) +{ + return 0; +} diff --git a/6515/libsensors_iio/SensorBase.h b/6515/libsensors_iio/SensorBase.h new file mode 100644 index 0000000..3a30310 --- /dev/null +++ b/6515/libsensors_iio/SensorBase.h @@ -0,0 +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_SENSOR_BASE_H +#define ANDROID_SENSOR_BASE_H + +#include <stdint.h> +#include <errno.h> +#include <sys/cdefs.h> +#include <sys/types.h> + +#if defined ANDROID_JELLYBEAN || defined ANDROID_KITKAT +//build for Jellybean or KitKat +#define LOGV_IF ALOGV_IF +#define LOGE_IF ALOGE_IF +#define LOGI_IF ALOGI_IF +#define LOGI ALOGI +#define LOGE ALOGE +#define LOGV ALOGV +#define LOGW ALOGW +#else +//build for ICS or earlier version +#endif + +#define FUNC_LOG \ + LOGV("%s", __PRETTY_FUNCTION__) +#define VFUNC_LOG \ + LOGV_IF(SensorBase::FUNC_ENTRY, \ + "Entering function '%s'", __PRETTY_FUNCTION__) +#define VHANDLER_LOG \ + LOGV_IF(SensorBase::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) + +/*****************************************************************************/ + +struct sensors_event_t; + +class SensorBase { +public: + /* Log enablers, each of these independent */ + static bool PROCESS_VERBOSE; /* process log messages */ + static bool EXTRA_VERBOSE; /* verbose log messages */ + static bool SYSFS_VERBOSE; /* log sysfs interactions as cat/echo for + repro purpose on a shell */ + /* Note that enabling this logs may affect performance */ + static bool FUNC_ENTRY; /* log entry in all one-time functions */ + static bool HANDLER_ENTRY; /* log entry in all handler functions */ + static bool ENG_VERBOSE; /* log a lot more info about the internals */ + static bool INPUT_DATA; /* log the data input from the events */ + static bool HANDLER_DATA; /* log the data fetched from the handlers */ + static bool DEBUG_BATCHING; /* log the data for debugging batching */ + +protected: + const char *dev_name; + const char *data_name; + char input_name[PATH_MAX]; + int dev_fd; + int data_fd; + + int openInput(const char* inputName); + static int64_t getTimestamp(); + static int64_t timevalToNano(timeval const& t) { + return t.tv_sec * 1000000000LL + t.tv_usec * 1000; + } + + int open_device(); + int close_device(); + +public: + SensorBase(const char* dev_name, const char* data_name); + virtual ~SensorBase(); + + virtual int readEvents(sensors_event_t* data, int count) = 0; + int readSample(long *data, int64_t *timestamp); + int readRawSample(float *data, int64_t *timestamp); + virtual bool hasPendingEvents() const; + virtual int getFd() const; + virtual int setDelay(int32_t handle, int64_t ns); + virtual int enable(int32_t handle, int enabled); + virtual int query(int what, int* value); + virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout); + virtual int flush(int handle); +}; + +/*****************************************************************************/ + +#endif // ANDROID_SENSOR_BASE_H diff --git a/6515/libsensors_iio/libmllite.so b/6515/libsensors_iio/libmllite.so Binary files differnew file mode 100644 index 0000000..407d4f2 --- /dev/null +++ b/6515/libsensors_iio/libmllite.so diff --git a/6515/libsensors_iio/libmplmpu.so b/6515/libsensors_iio/libmplmpu.so Binary files differnew file mode 100644 index 0000000..e1b0276 --- /dev/null +++ b/6515/libsensors_iio/libmplmpu.so diff --git a/6515/libsensors_iio/sensor_params.h b/6515/libsensors_iio/sensor_params.h new file mode 100644 index 0000000..d9bd978 --- /dev/null +++ b/6515/libsensors_iio/sensor_params.h @@ -0,0 +1,204 @@ +/* +* 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 + +/******************************************/ +/******************************************/ +//MPU9350 INV_COMPASS +#define COMPASS_MPU9350_RANGE (9830.f) +#define COMPASS_MPU9350_RESOLUTION (0.15f) +#define COMPASS_MPU9350_POWER (10.f) +#define COMPASS_MPU9350_MINDELAY (1000) +//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.5f) +#define ACCEL_MPU6500_MINDELAY (5000) +//ACCEL_ID_MPU9350 +#define ACCEL_MPU9350_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MPU9350_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_MPU9350_POWER (0.5f) +#define ACCEL_MPU9350_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.5f) +#define ACCEL_MPU9250_MINDELAY (5000) +//ACCEL_ID_MPU9150 +#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_MPU9150_POWER (0.5f) +#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 (5.5f) +#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 MPU9350 +#define GYRO_MPU9350_RANGE (2000.f * RAD_P_DEG) +#define GYRO_MPU9350_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_MPU9350_POWER (5.5f) +#define GYRO_MPU9350_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 (5000) +//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 (5000) +//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) +/******************************************/ +//PRESSURE BMP280 +#define PRESSURE_BMP280_RANGE (1100.f) // hpa +#define PRESSURE_BMP280_RESOLUTION (0.009995f)// in psi +#define PRESSURE_BMP280_POWER (0.004f) // 0.004mA +#define PRESSURE_BMP280_MINDELAY (33333) // 30Hz unit in ns +#endif /* INV_SENSOR_PARAMS_H */ + diff --git a/6515/libsensors_iio/sensors.h b/6515/libsensors_iio/sensors.h new file mode 100644 index 0000000..9920daf --- /dev/null +++ b/6515/libsensors_iio/sensors.h @@ -0,0 +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_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 ENABLE_PRESSURE + +enum { + ID_GY = 0, + ID_RG, + ID_A, + ID_M, + ID_RM, + ID_PS, + ID_O, + ID_RV, + ID_GRV, + ID_LA, + ID_GR, + ID_SM, + ID_P, + ID_SC, + ID_GMRV, + ID_SO +}; + +enum { + Gyro = 0, + RawGyro, + Accelerometer, + MagneticField, + RawMagneticField, + Pressure, + Orientation, + RotationVector, + GameRotationVector, + LinearAccel, + Gravity, + SignificantMotion, + StepDetector, + StepCounter, + GeomagneticRotationVector, + NumSensors +}; + +/* Physical parameters of the sensors supported by Invensense MPL */ +#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_RAW_MAGNETIC_FIELD_HANDLE (ID_RM) +#define SENSORS_PRESSURE_HANDLE (ID_PS) +#define SENSORS_ORIENTATION_HANDLE (ID_O) +#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV) +#define SENSORS_GAME_ROTATION_VECTOR_HANDLE (ID_GRV) +#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA) +#define SENSORS_GRAVITY_HANDLE (ID_GR) +#define SENSORS_SIGNIFICANT_MOTION_HANDLE (ID_SM) +#define SENSORS_PEDOMETER_HANDLE (ID_P) +#define SENSORS_STEP_COUNTER_HANDLE (ID_SC) +#define SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE (ID_GMRV) +#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO) + +/*****************************************************************************/ + +/* + Android KitKat + Populate sensor_t structure according to hardware sensors.h + { name, vendor, version, handle, type, maxRange, resolution, power, minDelay, + fifoReservedEventCount, fifoMaxEventCount, reserved[] } +*/ +#if defined ANDROID_KITKAT +static struct sensor_t sBaseSensorList[] = +{ + {"MPL Gyroscope", "Invensense", 1, + SENSORS_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 128, {}}, + {"MPL Raw Gyroscope", "Invensense", 1, + SENSORS_RAW_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, 0, 128, {}}, + {"MPL Accelerometer", "Invensense", 1, + SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}}, + {"MPL Magnetic Field", "Invensense", 1, + SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}}, + {"MPL Raw Magnetic Field", "Invensense", 1, + SENSORS_RAW_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}}, +#ifdef ENABLE_PRESSURE + {"MPL Pressure", "Invensense", 1, + SENSORS_PRESSURE_HANDLE, + SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, 0, 128, {}}, +#endif + {"MPL Orientation", "Invensense", 1, + SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, {}}, + {"MPL Rotation Vector", "Invensense", 1, + SENSORS_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}}, + {"MPL Game Rotation Vector", "Invensense", 1, + SENSORS_GAME_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 64, {}}, + {"MPL Linear Acceleration", "Invensense", 1, + SENSORS_LINEAR_ACCEL_HANDLE, + SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}}, + {"MPL Gravity", "Invensense", 1, + SENSORS_GRAVITY_HANDLE, + SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, {}}, + {"MPL Significant Motion", "Invensense", 1, + SENSORS_SIGNIFICANT_MOTION_HANDLE, + SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}}, + {"MPL Step Detector", "Invensense", 1, + SENSORS_PEDOMETER_HANDLE, + SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, 0, 128, {}}, + {"MPL Step Counter", "Invensense", 1, + SENSORS_STEP_COUNTER_HANDLE, + SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, 0, 0, {}}, + {"MPL Geomagnetic Rotation Vector", "Invensense", 1, + SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 5000, 0, 0, {}}, +#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, 0, 0, {}}, +#endif +}; +#else //ANDROID_KITKAT END +static struct sensor_t sBaseSensorList[] = +{ + {"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_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Accelerometer", "Invensense", 1, + SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Magnetic Field", "Invensense", 1, + SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Raw Magnetic Field", "Invensense", 1, + SENSORS_RAW_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, {}}, +#ifdef ENABLE_PRESSURE + {"MPL Pressure", "Invensense", 1, + SENSORS_PRESSURE_HANDLE, + SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, {}}, +#endif + {"MPL Orientation", "Invensense", 1, + SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, {}}, + {"MPL Rotation Vector", "Invensense", 1, + SENSORS_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Game Rotation Vector", "Invensense", 1, + SENSORS_GAME_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Linear Acceleration", "Invensense", 1, + SENSORS_LINEAR_ACCEL_HANDLE, + SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Gravity", "Invensense", 1, + SENSORS_GRAVITY_HANDLE, + SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Significant Motion", "Invensense", 1, + SENSORS_SIGNIFICANT_MOTION_HANDLE, + SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, {}}, + {"MPL Step Detector", "Invensense", 1, + SENSORS_PEDOMETER_HANDLE, + SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, {}}, + {"MPL Step Counter", "Invensense", 1, + SENSORS_STEP_COUNTER_HANDLE, + SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, {}}, + {"MPL Geomagnetic Rotation Vector", "Invensense", 1, + SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 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 +}; +#endif //ANDROID_JELLYBEAN END + +/*****************************************************************************/ + +/* + * 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) + +/* AKM compasses */ +#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) + +/* conversion of sensor rates */ +#define hertz_request = 200; +#define DEFAULT_MPL_GYRO_RATE (20000L) //us +#define DEFAULT_MPL_COMPASS_RATE (20000L) //us + +#define DEFAULT_HW_GYRO_RATE (100) //Hz +#define DEFAULT_HW_ACCEL_RATE (20) //ms +#define DEFAULT_HW_COMPASS_RATE (20000000L) //ns +#define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns + +/* convert ns to hardware units */ +#define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz +#define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms +#define HW_COMPASS_RATE_NS (rate_request) // to ns + +/* convert Hz to hardware units */ +#define HW_GYRO_RATE_HZ (hertz_request) +#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 +__END_DECLS + +#endif // ANDROID_SENSORS_H diff --git a/6515/libsensors_iio/sensors_mpl.cpp b/6515/libsensors_iio/sensors_mpl.cpp new file mode 100644 index 0000000..97a4009 --- /dev/null +++ b/6515/libsensors_iio/sensors_mpl.cpp @@ -0,0 +1,412 @@ +/* +* 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" + +/* + * 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 + */ + +/*****************************************************************************/ +/* The SENSORS Module */ + +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION +#define LOCAL_SENSORS (NumSensors + 1) +#else +#define LOCAL_SENSORS (NumSensors) +#endif + +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, + dso: NULL, + reserved: {0} + }, + get_sensors_list: sensors__get_sensors_list, +}; + +struct sensors_poll_context_t { + sensors_poll_device_1_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); + int query(int what, int *value); + int batch(int handle, int flags, int64_t period_ns, int64_t timeout); +#if defined ANDROID_KITKAT + int flush(int handle); +#endif + +private: + enum { + mpl = 0, + compass, + dmpOrient, + dmpSign, + dmpPed, + numSensorDrivers, // wake pipe goes here + numFds, + }; + + struct pollfd mPollFds[numFds]; + SensorBase *mSensor; + CompassSensor *mCompassSensor; + + static const size_t wake = numSensorDrivers; + static const char WAKE_MESSAGE = 'W'; + int mWritePipeFd; +}; + +/******************************************************************************/ + +sensors_poll_context_t::sensors_poll_context_t() { + VFUNC_LOG; + + /* TODO: Handle external pressure sensor */ + 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); + */ + + // 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; + + mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd(); + mPollFds[dmpSign].events = POLLPRI; + mPollFds[dmpSign].revents = 0; + + mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd(); + mPollFds[dmpPed].events = POLLPRI; + mPollFds[dmpPed].revents = 0; + + /* Timer based sensor initialization */ + int wakeFds[2]; + int result = pipe(wakeFds); + LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno)); + fcntl(wakeFds[0], F_SETFL, O_NONBLOCK); + fcntl(wakeFds[1], F_SETFL, O_NONBLOCK); + mWritePipeFd = wakeFds[1]; + + mPollFds[numSensorDrivers].fd = wakeFds[0]; + mPollFds[numSensorDrivers].events = POLLIN; + mPollFds[numSensorDrivers].revents = 0; +} + +sensors_poll_context_t::~sensors_poll_context_t() { + FUNC_LOG; + delete mSensor; + delete mCompassSensor; + for (int i = 0; i < numSensorDrivers; i++) { + close(mPollFds[i].fd); + } + close(mWritePipeFd); +} + +int sensors_poll_context_t::activate(int handle, int enabled) { + FUNC_LOG; + + int err; + err = mSensor->enable(handle, enabled); + if (!err) { + const char wakeMessage(WAKE_MESSAGE); + int result = write(mWritePipeFd, &wakeMessage, 1); + LOGE_IF(result < 0, + "error sending wake message (%s)", strerror(errno)); + } + return err; +} + +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; + + polltime = ((MPLSensor*) mSensor)->getStepCountPollTime(); + + // look for new events + nb = poll(mPollFds, numSensorDrivers, polltime); + LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime); + if (nb > 0) { + for (int i = 0; count && i < numSensorDrivers; i++) { + if (mPollFds[i].revents & (POLLIN | POLLPRI)) { + nb = 0; + if (i == mpl) { + ((MPLSensor*) mSensor)->buildMpuEvent(); + mPollFds[i].revents = 0; + } else if (i == compass) { + ((MPLSensor*) mSensor)->buildCompassEvent(); + mPollFds[i].revents = 0; + } else if (i == dmpOrient) { + nb = ((MPLSensor*)mSensor)-> + readDmpOrientEvents(data, count); + mPollFds[dmpOrient].revents= 0; + if (isDmpScreenAutoRotationEnabled() && nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } + } else if (i == dmpSign) { + nb = ((MPLSensor*) mSensor)-> + readDmpSignificantMotionEvents(data, count); + mPollFds[i].revents = 0; + count -= nb; + nbEvents += nb; + data += nb; + } else if (i == dmpPed) { + nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( + data, count, ID_P, 0); + mPollFds[i].revents = 0; + count -= nb; + nbEvents += nb; + data += nb; + } + if(nb == 0) { + nb = ((MPLSensor*) mSensor)->readEvents(data, count); + LOGI_IF(0, "sensors_mpl:readEvents() - " + "i=%d, nb=%d, count=%d, nbEvents=%d, " + "data->timestamp=%lld, data->data[0]=%f,", + i, nb, count, nbEvents, data->timestamp, + data->data[0]); + if (nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } + } + } + } + + /* to see if any step counter events */ + if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { + nb = 0; + nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( + data, count, ID_SC, 0); + LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " + "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ", + nb, count, nbEvents, data->timestamp); + if (nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } + } + } else if(nb == 0) { + /* to see if any step counter events */ + if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { + nb = 0; + nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( + data, count, ID_SC, 0); + LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " + "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ", + nb, count, nbEvents, data->timestamp); + if (nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } + } + + if (mPollFds[numSensorDrivers].revents & POLLIN) { + char msg; + int result = read(mPollFds[numSensorDrivers].fd, &msg, 1); + LOGE_IF(result < 0, + "error reading from wake pipe (%s)", strerror(errno)); + mPollFds[numSensorDrivers].revents = 0; + } + } + return nbEvents; +} + +int sensors_poll_context_t::query(int what, int* value) +{ + FUNC_LOG; + return mSensor->query(what, value); +} + +int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, + int64_t timeout) +{ + FUNC_LOG; + return mSensor->batch(handle, flags, period_ns, timeout); +} + +#if defined ANDROID_KITKAT +int sensors_poll_context_t::flush(int handle) +{ + FUNC_LOG; + return mSensor->flush(handle); +} +#endif + +/******************************************************************************/ + +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); +} + +static int poll__query(struct sensors_poll_device_1 *dev, + int what, int *value) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->query(what, value); +} + +static int poll__batch(struct sensors_poll_device_1 *dev, + int handle, int flags, int64_t period_ns, int64_t timeout) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->batch(handle, flags, period_ns, timeout); +} + +#if defined ANDROID_KITKAT +static int poll__flush(struct sensors_poll_device_1 *dev, + int handle) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->flush(handle); +} +#endif +/******************************************************************************/ + +/** 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_1)); + + dev->device.common.tag = HARDWARE_DEVICE_TAG; +#if defined ANDROID_KITKAT + dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_1; + dev->device.flush = poll__flush; +#else + dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_0; +#endif + 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; + dev->device.batch = poll__batch; + + *device = &dev->device.common; + status = 0; + + return status; +} diff --git a/6515/libsensors_iio/software/build/android/common.mk b/6515/libsensors_iio/software/build/android/common.mk new file mode 100644 index 0000000..acd3deb --- /dev/null +++ b/6515/libsensors_iio/software/build/android/common.mk @@ -0,0 +1,87 @@ +# Use bash for additional echo fancyness +SHELL = /bin/bash + +#################################################################################################### +## defines + +# Build for Jellybean +BUILD_ANDROID_JELLYBEAN = $(shell test -d $(ANDROID_ROOT)/frameworks/native && echo 1) + +## libraries ## +LIB_PREFIX = lib + +STATIC_LIB_EXT = a +SHARED_LIB_EXT = so + +# normally, overridden from outside +# ?= assignment sets it only if not already defined +TARGET ?= android + +MLLITE_LIB_NAME ?= mllite +MPL_LIB_NAME ?= mplmpu + +## applications ## +SHARED_APP_SUFFIX = -shared +STATIC_APP_SUFFIX = -static + +#################################################################################################### +## compile, includes, and linker + +ifeq ($(BUILD_ANDROID_JELLYBEAN),1) +ANDROID_COMPILE = -DANDROID_JELLYBEAN=1 +endif + +ANDROID_LINK = -nostdlib +ANDROID_LINK += -fpic +ANDROID_LINK += -Wl,--gc-sections +ANDROID_LINK += -Wl,--no-whole-archive +ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/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)/external/skia/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates + +KERNEL_INCLUDES = -I$(KERNEL_ROOT)/include + +INV_INCLUDES = -I$(INV_ROOT)/software/core/driver/include +INV_INCLUDES += -I$(MLLITE_DIR) +INV_INCLUDES += -I$(MLLITE_DIR)/linux + +INV_DEFINES += -DINV_CACHE_DMP=1 + +#################################################################################################### +## macros + +ifndef echo_in_colors +define echo_in_colors + echo -ne "\e[1;32m"$(1)"\e[0m" +endef +endif + + + diff --git a/6515/libsensors_iio/software/build/android/shared.mk b/6515/libsensors_iio/software/build/android/shared.mk new file mode 100644 index 0000000..ac225cb --- /dev/null +++ b/6515/libsensors_iio/software/build/android/shared.mk @@ -0,0 +1,76 @@ +SHELL=/bin/bash + +TARGET ?= android +PRODUCT ?= beagleboard +ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair +KERNEL_ROOT ?= /Android/trunk/0xdroid/kernel +MLSDK_ROOT ?= $(CURDIR) + +ifeq ($(VERBOSE),1) + DUMP=1>/dev/stdout +else + DUMP=1>/dev/null +endif + +include common.mk + +################################################################################ +## targets + +INV_ROOT = ../.. +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/self_test/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/playback/linux/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/devnode_parser/build/$(TARGET) + +INSTALL_DIR = $(CURDIR) + +################################################################################ +## macros + +define echo_in_colors + echo -ne "\e[1;34m"$(1)"\e[0m" +endef + +################################################################################ +## rules + +.PHONY : all mllite mpl clean + +all: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + +clean: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + +cleanall: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + +install: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + diff --git a/6515/libsensors_iio/software/core/driver/include/log.h b/6515/libsensors_iio/software/core/driver/include/log.h new file mode 100644 index 0000000..626b00e --- /dev/null +++ b/6515/libsensors_iio/software/core/driver/include/log.h @@ -0,0 +1,368 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/* + * This file incorporates work covered by the following copyright and + * permission notice: + * + * Copyright (C) 2005 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. + */ + +/* + * C/C++ logging functions. See the logging documentation for API details. + * + * We'd like these to be available from C code (in case we import some from + * somewhere), so this has a C interface. + * + * The output will be correct when the log file is shared between multiple + * threads and/or multiple processes so long as the operating system + * supports O_APPEND. These calls have mutex-protected data structures + * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. + */ +#ifndef _LIBS_CUTILS_MPL_LOG_H +#define _LIBS_CUTILS_MPL_LOG_H + +#include <stdlib.h> +#include <stdarg.h> + +#ifdef ANDROID +#ifdef NDK_BUILD +#include "log_macros.h" +#else +#include <utils/Log.h> /* For the LOG macro */ +#endif +#endif + +#ifdef __KERNEL__ +#include <linux/kernel.h> +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined ANDROID_JELLYBEAN || defined ANDROID_KITKAT +#define LOG ALOG +#define LOG_ERRROR ANDROID_LOG_ERROR +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Normally we strip MPL_LOGV (VERBOSE messages) from release builds. + * You can modify this (for example with "#define MPL_LOG_NDEBUG 0" + * at the top of your source file) to change that behavior. + */ +#ifndef MPL_LOG_NDEBUG +#ifdef NDEBUG +#define MPL_LOG_NDEBUG 1 +#else +#define MPL_LOG_NDEBUG 0 +#endif +#endif + +#ifdef __KERNEL__ +#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE +#define MPL_LOG_DEFAULT KERN_DEFAULT +#define MPL_LOG_VERBOSE KERN_CONT +#define MPL_LOG_DEBUG KERN_NOTICE +#define MPL_LOG_INFO KERN_INFO +#define MPL_LOG_WARN KERN_WARNING +#define MPL_LOG_ERROR KERN_ERR +#define MPL_LOG_SILENT MPL_LOG_VERBOSE + +#else + /* Based off the log priorities in android + /system/core/include/android/log.h */ +#define MPL_LOG_UNKNOWN (0) +#define MPL_LOG_DEFAULT (1) +#define MPL_LOG_VERBOSE (2) +#define MPL_LOG_DEBUG (3) +#define MPL_LOG_INFO (4) +#define MPL_LOG_WARN (5) +#define MPL_LOG_ERROR (6) +#define MPL_LOG_SILENT (8) +#endif + + +/* + * This is the local tag used for the following simplified + * logging macros. You can change this preprocessor definition + * before using the other macros to change the tag. + */ +#ifndef MPL_LOG_TAG +#ifdef __KERNEL__ +#define MPL_LOG_TAG +#else +#define MPL_LOG_TAG NULL +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Simplified macro to send a verbose log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGV +#if MPL_LOG_NDEBUG +#ifdef _WIN32 +#define MPL_LOGV(fmt, ...) \ + do { \ + __pragma (warning(suppress : 4127 )) \ + if (0) \ + MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ + __pragma (warning(suppress : 4127 )) \ + } while (0) +#else +#define MPL_LOGV(fmt, ...) \ + do { \ + if (0) \ + MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ + } while (0) +#endif + +#else +#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef CONDITION +#define CONDITION(cond) ((cond) != 0) +#endif + +#ifndef MPL_LOGV_IF +#if MPL_LOG_NDEBUG +#define MPL_LOGV_IF(cond, fmt, ...) \ + do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0) +#else +#define MPL_LOGV_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif +#endif + +/* + * Simplified macro to send a debug log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGD +#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif + +#ifndef MPL_LOGD_IF +#define MPL_LOGD_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send an info log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGI +#ifdef __KERNEL__ +#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGI_IF +#define MPL_LOGI_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send a warning log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGW +#ifdef __KERNEL__ +#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGW_IF +#define MPL_LOGW_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send an error log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGE +#ifdef __KERNEL__ +#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGE_IF +#define MPL_LOGE_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Log a fatal error. If the given condition fails, this stops program + * execution like a normal assertion, but also generating the given message. + * It is NOT stripped from release builds. Note that the condition test + * is -inverted- from the normal assert() semantics. + */ +#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \ + fmt, ##__VA_ARGS__)) \ + : (void)0) + +#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \ + (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__))) + +/* + * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that + * are stripped out of release builds. + */ +#if MPL_LOG_NDEBUG +#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ + do { \ + if (0) \ + MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \ + } while (0) +#define MPL_LOG_FATAL(fmt, ...) \ + do { \ + if (0) \ + MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \ + } while (0) +#else +#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ + MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__) +#define MPL_LOG_FATAL(fmt, ...) \ + MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) +#endif + +/* + * Assertion that generates a log message when the assertion fails. + * Stripped out of release builds. Uses the current MPL_LOG_TAG. + */ +#define MPL_LOG_ASSERT(cond, fmt, ...) \ + MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__) + +/* --------------------------------------------------------------------- */ + +/* + * Basic log message macro. + * + * Example: + * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno); + * + * The second argument may be NULL or "" to indicate the "global" tag. + */ +#ifndef MPL_LOG +#define MPL_LOG(priority, tag, fmt, ...) \ + MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__) +#endif + +/* + * Log macro that allows you to specify a number for the priority. + */ +#ifndef MPL_LOG_PRI +#ifdef ANDROID +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + LOG(priority, tag, fmt, ##__VA_ARGS__) +#elif defined __KERNEL__ +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__) +#else +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__) +#endif +#endif + +/* + * Log macro that allows you to pass in a varargs ("args" is a va_list). + */ +#ifndef MPL_LOG_PRI_VA +#ifdef ANDROID +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + android_vprintLog(priority, NULL, tag, fmt, args) +#elif defined __KERNEL__ +/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */ +#else +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + _MLPrintVaLog(priority, NULL, tag, fmt, args) +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * =========================================================================== + * + * The stuff in the rest of this file should not be used directly. + */ + +#ifndef ANDROID +int _MLPrintLog(int priority, const char *tag, const char *fmt, ...); +int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args); +/* Final implementation of actual writing to a character device */ +int _MLWriteLog(const char *buf, int buflen); +#endif + +static inline void __print_result_location(int result, + const char *file, + const char *func, int line) +{ + MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result); +} + +#ifdef _WIN32 +/* The pragma removes warning about expression being constant */ +#define LOG_RESULT_LOCATION(condition) \ + do { \ + __print_result_location((int)(condition), __FILE__, \ + __func__, __LINE__); \ + __pragma (warning(suppress : 4127 )) \ + } while (0) +#else +#define LOG_RESULT_LOCATION(condition) \ + do { \ + __print_result_location((int)(condition), __FILE__, \ + __func__, __LINE__); \ + } while (0) +#endif + + +#define INV_ERROR_CHECK(r_1329) \ + if (r_1329) { \ + LOG_RESULT_LOCATION(r_1329); \ + return r_1329; \ + } + +#ifdef __cplusplus +} +#endif +#endif /* _LIBS_CUTILS_MPL_LOG_H */ diff --git a/6515/libsensors_iio/software/core/driver/include/mlinclude.h b/6515/libsensors_iio/software/core/driver/include/mlinclude.h new file mode 100644 index 0000000..9725199 --- /dev/null +++ b/6515/libsensors_iio/software/core/driver/include/mlinclude.h @@ -0,0 +1,38 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef INV_INCLUDE_H__ +#define INV_INCLUDE_H__ + +#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere + +#ifdef COVERAGE +#include "utestCommon.h" +#endif +#ifdef PROFILE +#include "profile.h" +#endif + +#ifdef WIN32 +#ifdef COVERAGE + +extern int functionEnterLog(const char *file, const char *func); +extern int functionExitLog(const char *file, const char *func); + +#undef INVENSENSE_FUNC_START +#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \ + int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__) +#endif // COVERAGE +#endif // WIN32 + +#ifdef PROFILE +#undef INVENSENSE_FUNC_START +#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__) +#define return if ( profileExit(__FILE__, __FUNCTION__) ) return +#endif // PROFILE + +// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return + +#endif //INV_INCLUDE_H__ diff --git a/6515/libsensors_iio/software/core/driver/include/mlmath.h b/6515/libsensors_iio/software/core/driver/include/mlmath.h new file mode 100644 index 0000000..37194d6 --- /dev/null +++ b/6515/libsensors_iio/software/core/driver/include/mlmath.h @@ -0,0 +1,95 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef _ML_MATH_H_ +#define _ML_MATH_H_ + +#ifndef MLMATH +// This define makes Microsoft pickup things like M_PI +#define _USE_MATH_DEFINES +#include <math.h> + +#ifdef WIN32 +// Microsoft doesn't follow standards +#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5)))) +#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) +#endif + +#else // MLMATH + +#ifdef __cplusplus +extern "C" { +#endif +/* MPL needs below functions */ +double ml_asin(double); +double ml_atan(double); +double ml_atan2(double, double); +double ml_log(double); +double ml_sqrt(double); +double ml_ceil(double); +double ml_floor(double); +double ml_cos(double); +double ml_sin(double); +double ml_acos(double); +#ifdef __cplusplus +} // extern "C" +#endif + +/* + * We rename functions here to provide the hook for other + * customized math functions. + */ +#define sqrt(x) ml_sqrt(x) +#define log(x) ml_log(x) +#define asin(x) ml_asin(x) +#define atan(x) ml_atan(x) +#define atan2(x,y) ml_atan2(x,y) +#define ceil(x) ml_ceil(x) +#define floor(x) ml_floor(x) +#define fabs(x) (((x)<0)?-(x):(x)) +#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5)))) +#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) +#define cos(x) ml_cos(x) +#define sin(x) ml_sin(x) +#define acos(x) ml_acos(x) + +#define pow(x,y) ml_pow(x,y) + +#ifdef LINUX +/* stubs for float version of math functions */ +#define cosf(x) ml_cos(x) +#define sinf(x) ml_sin(x) +#define atan2f(x,y) ml_atan2(x,y) +#define sqrtf(x) ml_sqrt(x) +#endif + + + +#endif // MLMATH + +#ifndef M_PI +#define M_PI 3.14159265358979 +#endif + +#ifndef ABS +#define ABS(x) (((x)>=0)?(x):-(x)) +#endif + +#ifndef MIN +#define MIN(x,y) (((x)<(y))?(x):(y)) +#endif + +#ifndef MAX +#define MAX(x,y) (((x)>(y))?(x):(y)) +#endif + +/*---------------------------*/ +#endif /* !_ML_MATH_H_ */ diff --git a/6515/libsensors_iio/software/core/driver/include/mlsl.h b/6515/libsensors_iio/software/core/driver/include/mlsl.h new file mode 100644 index 0000000..12f2901 --- /dev/null +++ b/6515/libsensors_iio/software/core/driver/include/mlsl.h @@ -0,0 +1,283 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef __MLSL_H__ +#define __MLSL_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @defgroup MLSL + * @brief Motion Library - Serial Layer. + * The Motion Library System Layer provides the Motion Library + * with the communication interface to the hardware. + * + * The communication interface is assumed to support serial + * transfers in burst of variable length up to + * SERIAL_MAX_TRANSFER_SIZE. + * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes. + * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will + * be subdivided in smaller transfers of length <= + * SERIAL_MAX_TRANSFER_SIZE. + * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to + * overcome any host processor transfer size limitation down to + * 1 B, the minimum. + * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor + * performance and efficiency while requiring higher resource usage + * (mostly buffering). A smaller value will increase overhead and + * decrease efficiency but allows to operate with more resource + * constrained processor and master serial controllers. + * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the + * mlsl.h header file and master serial controllers. + * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the + * mlsl.h header file. + * + * @{ + * @file mlsl.h + * @brief The Motion Library System Layer. + * + */ + +/* + * NOTE : to properly support Yamaha compass reads, + * the max transfer size should be at least 9 B. + * Length in bytes, typically a power of 2 >= 2 + */ +#define SERIAL_MAX_TRANSFER_SIZE 31 + +#ifndef __KERNEL__ +/** + * inv_serial_open() - used to open the serial port. + * @port The COM port specification associated with the device in use. + * @sl_handle a pointer to the file handle to the serial device to be open + * for the communication. + * This port is used to send and receive data to the device. + * + * This function is called by inv_serial_start(). + * Unlike previous MPL Software releases, explicitly calling + * inv_serial_start() is mandatory to instantiate the communication + * with the device. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_open(char const *port, void **sl_handle); + +/** + * inv_serial_close() - used to close the serial port. + * @sl_handle a file handle to the serial device used for the communication. + * + * This port is used to send and receive data to the device. + * + * This function is called by inv_serial_stop(). + * Unlike previous MPL Software releases, explicitly calling + * inv_serial_stop() is mandatory to properly shut-down the + * communication with the device. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_close(void *sl_handle); + +/** + * inv_serial_reset() - used to reset any buffering the driver may be doing + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_reset(void *sl_handle); +#endif + +/** + * inv_serial_single_write() - used to write a single byte of data. + * @sl_handle pointer to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to write. + * @data Single byte of data to write. + * + * It is called by the MPL to write a single byte of data to the MPU. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_single_write( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned char data); + +/** + * inv_serial_write() - used to write multiple bytes of data to registers. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to write. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_write( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char const *data); + +/** + * inv_serial_read() - used to read multiple bytes of data from registers. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to read. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_read( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_read_mem() - used to read multiple bytes of data from the memory. + * This should be sent by I2C or SPI. + * + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @mem_addr The location in the memory to read from. + * @length Length of burst data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_read_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned char bank_reg, + unsigned char addr_reg, + unsigned char mem_reg, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_write_mem() - used to write multiple bytes of data to the memory. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @mem_addr The location in the memory to write to. + * @length Length of burst data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_write_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned char bank_reg, + unsigned char addr_reg, + unsigned char mem_reg, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_read_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned char fifo_reg, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_write_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned char fifo_reg, + unsigned short length, + unsigned char const *data); + +#ifndef __KERNEL__ +/** + * inv_serial_read_cfg() - used to get the configuration data. + * @cfg Pointer to the configuration data. + * @len Length of the configuration data. + * + * Is called by the MPL to get the configuration data + * used by the motion library. + * This data would typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_read_cfg(unsigned char *cfg, unsigned int len); + +/** + * inv_serial_write_cfg() - used to save the configuration data. + * @cfg Pointer to the configuration data. + * @len Length of the configuration data. + * + * Is called by the MPL to save the configuration data used by the + * motion library. + * This data would typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_write_cfg(unsigned char *cfg, unsigned int len); + +/** + * inv_serial_read_cal() - used to get the calibration data. + * @cfg Pointer to the calibration data. + * @len Length of the calibration data. + * + * It is called by the MPL to get the calibration data used by the + * motion library. + * This data is typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_read_cal(unsigned char *cal, unsigned int len); + +/** + * inv_serial_write_cal() - used to save the calibration data. + * + * @cfg Pointer to the calibration data. + * @len Length of the calibration data. + * + * It is called by the MPL to save the calibration data used by the + * motion library. + * This data is typically be saved in non-volatile memory. + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_write_cal(unsigned char *cal, unsigned int len); + +/** + * inv_serial_get_cal_length() - Get the calibration length from the storage. + * @len lenght to be returned + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_get_cal_length(unsigned int *len); +#endif +#ifdef __cplusplus +} +#endif +/** + * @} + */ +#endif /* __MLSL_H__ */ diff --git a/6515/libsensors_iio/software/core/driver/include/mltypes.h b/6515/libsensors_iio/software/core/driver/include/mltypes.h new file mode 100644 index 0000000..09eccce --- /dev/null +++ b/6515/libsensors_iio/software/core/driver/include/mltypes.h @@ -0,0 +1,235 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/** + * @defgroup MLERROR + * @brief Motion Library - Error definitions. + * Definition of the error codes used within the MPL and + * returned to the user. + * Every function tries to return a meaningful error code basing + * on the occuring error condition. The error code is numeric. + * + * The available error codes and their associated values are: + * - (0) INV_SUCCESS + * - (32) INV_ERROR + * - (22 / EINVAL) INV_ERROR_INVALID_PARAMETER + * - (1 / EPERM) INV_ERROR_FEATURE_NOT_ENABLED + * - (36) INV_ERROR_FEATURE_NOT_IMPLEMENTED + * - (38) INV_ERROR_DMP_NOT_STARTED + * - (39) INV_ERROR_DMP_STARTED + * - (40) INV_ERROR_NOT_OPENED + * - (41) INV_ERROR_OPENED + * - (19 / ENODEV) INV_ERROR_INVALID_MODULE + * - (12 / ENOMEM) INV_ERROR_MEMORY_EXAUSTED + * - (44) INV_ERROR_DIVIDE_BY_ZERO + * - (45) INV_ERROR_ASSERTION_FAILURE + * - (46) INV_ERROR_FILE_OPEN + * - (47) INV_ERROR_FILE_READ + * - (48) INV_ERROR_FILE_WRITE + * - (49) INV_ERROR_INVALID_CONFIGURATION + * - (52) INV_ERROR_SERIAL_CLOSED + * - (53) INV_ERROR_SERIAL_OPEN_ERROR + * - (54) INV_ERROR_SERIAL_READ + * - (55) INV_ERROR_SERIAL_WRITE + * - (56) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED + * - (57) INV_ERROR_SM_TRANSITION + * - (58) INV_ERROR_SM_IMPROPER_STATE + * - (62) INV_ERROR_FIFO_OVERFLOW + * - (63) INV_ERROR_FIFO_FOOTER + * - (64) INV_ERROR_FIFO_READ_COUNT + * - (65) INV_ERROR_FIFO_READ_DATA + * - (72) INV_ERROR_MEMORY_SET + * - (82) INV_ERROR_LOG_MEMORY_ERROR + * - (83) INV_ERROR_LOG_OUTPUT_ERROR + * - (92) INV_ERROR_OS_BAD_PTR + * - (93) INV_ERROR_OS_BAD_HANDLE + * - (94) INV_ERROR_OS_CREATE_FAILED + * - (95) INV_ERROR_OS_LOCK_FAILED + * - (102) INV_ERROR_COMPASS_DATA_OVERFLOW + * - (103) INV_ERROR_COMPASS_DATA_UNDERFLOW + * - (104) INV_ERROR_COMPASS_DATA_NOT_READY + * - (105) INV_ERROR_COMPASS_DATA_ERROR + * - (107) INV_ERROR_CALIBRATION_LOAD + * - (108) INV_ERROR_CALIBRATION_STORE + * - (109) INV_ERROR_CALIBRATION_LEN + * - (110) INV_ERROR_CALIBRATION_CHECKSUM + * - (111) INV_ERROR_ACCEL_DATA_OVERFLOW + * - (112) INV_ERROR_ACCEL_DATA_UNDERFLOW + * - (113) INV_ERROR_ACCEL_DATA_NOT_READY + * - (114) INV_ERROR_ACCEL_DATA_ERROR + * + * The available warning codes and their associated values are: + * - (115) INV_WARNING_MOTION_RACE + * - (116) INV_WARNING_QUAT_TRASHED + * + * @{ + * @file mltypes.h + * @} + */ + +#ifndef MLTYPES_H +#define MLTYPES_H + +#ifdef __KERNEL__ +#include <linux/types.h> +#include <asm-generic/errno-base.h> +#else +#include "stdint_invensense.h" +#include <errno.h> +#endif +#include <limits.h> + +#ifndef REMOVE_INV_ERROR_T +/*--------------------------- + * ML Types + *--------------------------*/ + +/** + * @struct inv_error_t mltypes.h "mltypes" + * @brief The MPL Error Code return type. + * + * @code + * typedef unsigned char inv_error_t; + * @endcode + */ +//typedef unsigned char inv_error_t; +typedef int inv_error_t; +#endif + +typedef long long inv_time_t; + +#if !defined __GNUC__ && !defined __KERNEL__ +typedef int8_t __s8; +typedef int16_t __s16; +typedef int32_t __s32; +typedef int32_t __s64; + +typedef uint8_t __u8; +typedef uint16_t __u16; +typedef uint32_t __u32; +typedef uint64_t __u64; +#elif !defined __KERNEL__ +#include <sys/types.h> +#endif + +#ifndef __cplusplus +#ifndef __KERNEL__ +typedef int_fast8_t bool; + +#ifndef false +#define false 0 +#endif + +#ifndef true +#define true 1 +#endif + +#endif +#endif + +/*--------------------------- + * ML Defines + *--------------------------*/ + +#ifndef NULL +#define NULL 0 +#endif + +#ifndef __KERNEL__ +#ifndef ARRAY_SIZE +/* Dimension of an array */ +#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0])) +#endif +#endif +/* - ML Errors. - */ +#define ERROR_NAME(x) (#x) +#define ERROR_CHECK_FIRST(first, x) \ + { if (INV_SUCCESS == first) first = x; } + +#define INV_SUCCESS (0) +/* Generic Error code. Proprietary Error Codes only */ +#define INV_ERROR_BASE (0x20) +#define INV_ERROR (INV_ERROR_BASE) + +/* Compatibility and other generic error codes */ +#define INV_ERROR_INVALID_PARAMETER (EINVAL) +#define INV_ERROR_FEATURE_NOT_ENABLED (EPERM) +#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (INV_ERROR_BASE + 4) +#define INV_ERROR_DMP_NOT_STARTED (INV_ERROR_BASE + 6) +#define INV_ERROR_DMP_STARTED (INV_ERROR_BASE + 7) +#define INV_ERROR_NOT_OPENED (INV_ERROR_BASE + 8) +#define INV_ERROR_OPENED (INV_ERROR_BASE + 9) +#define INV_ERROR_INVALID_MODULE (ENODEV) +#define INV_ERROR_MEMORY_EXAUSTED (ENOMEM) +#define INV_ERROR_DIVIDE_BY_ZERO (INV_ERROR_BASE + 12) +#define INV_ERROR_ASSERTION_FAILURE (INV_ERROR_BASE + 13) +#define INV_ERROR_FILE_OPEN (INV_ERROR_BASE + 14) +#define INV_ERROR_FILE_READ (INV_ERROR_BASE + 15) +#define INV_ERROR_FILE_WRITE (INV_ERROR_BASE + 16) +#define INV_ERROR_INVALID_CONFIGURATION (INV_ERROR_BASE + 17) +#define INV_ERROR_NOT_AUTHORIZED (INV_ERROR_BASE + 18) + +/* Serial Communication */ +#define INV_ERROR_SERIAL_CLOSED (INV_ERROR_BASE + 20) +#define INV_ERROR_SERIAL_OPEN_ERROR (INV_ERROR_BASE + 21) +#define INV_ERROR_SERIAL_READ (INV_ERROR_BASE + 22) +#define INV_ERROR_SERIAL_WRITE (INV_ERROR_BASE + 23) +#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (INV_ERROR_BASE + 24) + +/* SM = State Machine */ +#define INV_ERROR_SM_TRANSITION (INV_ERROR_BASE + 25) +#define INV_ERROR_SM_IMPROPER_STATE (INV_ERROR_BASE + 26) + +/* Fifo */ +#define INV_ERROR_FIFO_OVERFLOW (INV_ERROR_BASE + 30) +#define INV_ERROR_FIFO_FOOTER (INV_ERROR_BASE + 31) +#define INV_ERROR_FIFO_READ_COUNT (INV_ERROR_BASE + 32) +#define INV_ERROR_FIFO_READ_DATA (INV_ERROR_BASE + 33) + +/* Memory & Registers, Set & Get */ +#define INV_ERROR_MEMORY_SET (INV_ERROR_BASE + 40) + +#define INV_ERROR_LOG_MEMORY_ERROR (INV_ERROR_BASE + 50) +#define INV_ERROR_LOG_OUTPUT_ERROR (INV_ERROR_BASE + 51) + +/* OS interface errors */ +#define INV_ERROR_OS_BAD_PTR (INV_ERROR_BASE + 60) +#define INV_ERROR_OS_BAD_HANDLE (INV_ERROR_BASE + 61) +#define INV_ERROR_OS_CREATE_FAILED (INV_ERROR_BASE + 62) +#define INV_ERROR_OS_LOCK_FAILED (INV_ERROR_BASE + 63) + +/* Compass errors */ +#define INV_ERROR_COMPASS_DATA_OVERFLOW (INV_ERROR_BASE + 70) +#define INV_ERROR_COMPASS_DATA_UNDERFLOW (INV_ERROR_BASE + 71) +#define INV_ERROR_COMPASS_DATA_NOT_READY (INV_ERROR_BASE + 72) +#define INV_ERROR_COMPASS_DATA_ERROR (INV_ERROR_BASE + 73) + +/* Load/Store calibration */ +#define INV_ERROR_CALIBRATION_LOAD (INV_ERROR_BASE + 75) +#define INV_ERROR_CALIBRATION_STORE (INV_ERROR_BASE + 76) +#define INV_ERROR_CALIBRATION_LEN (INV_ERROR_BASE + 77) +#define INV_ERROR_CALIBRATION_CHECKSUM (INV_ERROR_BASE + 78) + +/* Accel errors */ +#define INV_ERROR_ACCEL_DATA_OVERFLOW (INV_ERROR_BASE + 79) +#define INV_ERROR_ACCEL_DATA_UNDERFLOW (INV_ERROR_BASE + 80) +#define INV_ERROR_ACCEL_DATA_NOT_READY (INV_ERROR_BASE + 81) +#define INV_ERROR_ACCEL_DATA_ERROR (INV_ERROR_BASE + 82) + +/* No Motion Warning States */ +#define INV_WARNING_MOTION_RACE (INV_ERROR_BASE + 83) +#define INV_WARNING_QUAT_TRASHED (INV_ERROR_BASE + 84) +#define INV_WARNING_GYRO_MAG (INV_ERROR_BASE + 85) + +#define INV_WARNING_SEMAPHORE_TIMEOUT (INV_ERROR_BASE + 86) + + +/* For Linux coding compliance */ +#ifndef __KERNEL__ +#define EXPORT_SYMBOL(x) +#endif + +#endif /* MLTYPES_H */ diff --git a/6515/libsensors_iio/software/core/driver/include/stdint_invensense.h b/6515/libsensors_iio/software/core/driver/include/stdint_invensense.h new file mode 100644 index 0000000..9440228 --- /dev/null +++ b/6515/libsensors_iio/software/core/driver/include/stdint_invensense.h @@ -0,0 +1,41 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef STDINT_INVENSENSE_H +#define STDINT_INVENSENSE_H + +#ifndef _WIN32 + +#ifdef __KERNEL__ +#include <linux/types.h> +#else +#include <stdint.h> +#endif + +#else + +#include <windows.h> + +typedef signed char int8_t; +typedef short int16_t; +typedef long int32_t; +typedef long long int64_t; + +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned long uint32_t; +typedef unsigned long long uint64_t; + +typedef int int_fast8_t; +typedef int int_fast16_t; +typedef long int_fast32_t; + +typedef unsigned int uint_fast8_t; +typedef unsigned int uint_fast16_t; +typedef unsigned long uint_fast32_t; + +#endif + +#endif // STDINT_INVENSENSE_H diff --git a/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so Binary files differnew file mode 100755 index 0000000..407d4f2 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/build/android/libmllite.so diff --git a/6515/libsensors_iio/software/core/mllite/build/android/shared.mk b/6515/libsensors_iio/software/core/mllite/build/android/shared.mk new file mode 100644 index 0000000..1418450 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/build/android/shared.mk @@ -0,0 +1,87 @@ +MLLITE_LIB_NAME = mllite +LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +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 +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(INV_ROOT)/simple_apps/common +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -shared +LFLAGS += -Wl,-soname,$(LIBRARY) +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 + +#################################################################################################### +## sources + +#INV_SOURCES provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all mllite clean cleanall makefiles + +all: mllite + +mllite: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") + $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/6515/libsensors_iio/software/core/mllite/build/filelist.mk b/6515/libsensors_iio/software/core/mllite/build/filelist.mk new file mode 100644 index 0000000..011120c --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/build/filelist.mk @@ -0,0 +1,42 @@ +#### filelist.mk for mllite #### + +# headers only +HEADERS := $(MLLITE_DIR)/invensense.h + +# headers for sources +HEADERS += $(MLLITE_DIR)/data_builder.h +HEADERS += $(MLLITE_DIR)/hal_outputs.h +HEADERS += $(MLLITE_DIR)/message_layer.h +HEADERS += $(MLLITE_DIR)/ml_math_func.h +HEADERS += $(MLLITE_DIR)/mpl.h +HEADERS += $(MLLITE_DIR)/results_holder.h +HEADERS += $(MLLITE_DIR)/start_manager.h +HEADERS += $(MLLITE_DIR)/storage_manager.h + +# headers (linux specific) +HEADERS += $(MLLITE_DIR)/linux/mlos.h +HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h +HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h +HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h + +# sources +SOURCES := $(MLLITE_DIR)/data_builder.c +SOURCES += $(MLLITE_DIR)/hal_outputs.c +SOURCES += $(MLLITE_DIR)/message_layer.c +SOURCES += $(MLLITE_DIR)/ml_math_func.c +SOURCES += $(MLLITE_DIR)/mpl.c +SOURCES += $(MLLITE_DIR)/results_holder.c +SOURCES += $(MLLITE_DIR)/start_manager.c +SOURCES += $(MLLITE_DIR)/storage_manager.c + +# sources (linux specific) +SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c +SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c +SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c +SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c + + +INV_SOURCES += $(SOURCES) + +VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux + diff --git a/6515/libsensors_iio/software/core/mllite/data_builder.c b/6515/libsensors_iio/software/core/mllite/data_builder.c new file mode 100644 index 0000000..0c842a1 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/data_builder.c @@ -0,0 +1,1711 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/** + * @defgroup Data_Builder data_builder + * @brief Motion Library - Data Builder + * Constructs and Creates the data for MPL + * + * @{ + * @file data_builder.c + * @brief Data Builder. + */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 1 /* 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 +#define MPL_LOG_TAG "MLLITE" + +typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); + +struct process_t { + inv_process_cb_func func; + int priority; + int data_required; +}; + +struct inv_data_builder_t { + int num_cb; + struct process_t process[INV_MAX_DATA_CB]; + struct inv_db_save_t save; + struct inv_db_save_mpl_t save_mpl; + struct inv_db_save_accel_mpl_t save_accel_mpl; + int compass_disturbance; + int mode; +#ifdef INV_PLAYBACK_DBG + int debug_mode; + int last_mode; + FILE *file; +#endif +}; + +void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); +static void inv_set_contiguous(void); + +static struct inv_data_builder_t inv_data_builder; +static struct inv_sensor_cal_t sensors; + +#ifdef INV_PLAYBACK_DBG + +/** Turn on data logging to allow playback of same scenario at a later time. +* @param[in] file File to write to, must be open. +*/ +void inv_turn_on_data_logging(FILE *file) +{ + MPL_LOGV("input data logging started\n"); + inv_data_builder.file = file; + inv_data_builder.debug_mode = RD_RECORD; +} + +/** Turn off data logging to allow playback of same scenario at a later time. +* File passed to inv_turn_on_data_logging() must be closed after calling this. +*/ +void inv_turn_off_data_logging() +{ + MPL_LOGV("input data logging stopped\n"); + inv_data_builder.debug_mode = RD_NO_DEBUG; + inv_data_builder.file = NULL; +} +#endif + +/** Gets last value of raw compass data. +* @param[out] raw Raw compass data in mounting frame in hardware units. Length 3. +*/ +void inv_get_raw_compass(short *raw) +{ + memcpy(raw, sensors.compass.raw, sizeof(sensors.compass.raw)); +} + +/** 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(&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.accel.accuracy == 3) { + inv_set_accel_bias_found(1); + } + if (sensors.compass.accuracy == 3) { + inv_set_compass_bias_found(1); + } + return INV_SUCCESS; +} + +/** 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, &inv_data_builder.save, sizeof(inv_data_builder.save)); + return INV_SUCCESS; +} + +/** This function receives the data for mpl that was stored in non-volatile memory between power off */ +static inv_error_t inv_db_load_mpl_func(const unsigned char *data) +{ + memcpy(&inv_data_builder.save_mpl, data, sizeof(inv_data_builder.save_mpl)); + + return INV_SUCCESS; +} + +/** This function returns the data for mpl to be stored in non-volatile memory between power off */ +static inv_error_t inv_db_save_mpl_func(unsigned char *data) +{ + memcpy(data, &inv_data_builder.save_mpl, sizeof(inv_data_builder.save_mpl)); + return INV_SUCCESS; +} + +/** This function receives the data for mpl that was stored in non-volatile memory between power off */ +static inv_error_t inv_db_load_accel_mpl_func(const unsigned char *data) +{ + memcpy(&inv_data_builder.save_accel_mpl, data, sizeof(inv_data_builder.save_accel_mpl)); + + return INV_SUCCESS; +} + +/** This function returns the data for mpl to be stored in non-volatile memory between power off */ +static inv_error_t inv_db_save_accel_mpl_func(unsigned char *data) +{ + memcpy(data, &inv_data_builder.save_accel_mpl, sizeof(inv_data_builder.save_accel_mpl)); + return INV_SUCCESS; +} + +/** Initialize the data builder +*/ +inv_error_t inv_init_data_builder(void) +{ + /* TODO: Hardcode temperature scale/offset here. */ + memset(&inv_data_builder, 0, sizeof(inv_data_builder)); + memset(&sensors, 0, sizeof(sensors)); + + // disable the soft iron transform process + inv_reset_compass_soft_iron_matrix(); + + return ((inv_register_load_store(inv_db_load_func, inv_db_save_func, + sizeof(inv_data_builder.save), + INV_DB_SAVE_KEY)) + | (inv_register_load_store(inv_db_load_mpl_func, inv_db_save_mpl_func, + sizeof(inv_data_builder.save_mpl), + INV_DB_SAVE_MPL_KEY)) + | (inv_register_load_store(inv_db_load_accel_mpl_func, inv_db_save_accel_mpl_func, + sizeof(inv_data_builder.save_accel_mpl), + INV_DB_SAVE_ACCEL_MPL_KEY)) ); +} + +/** Gyro sensitivity. +* @return A scale factor to convert device units to degrees per second scaled by 2^16 +* such that degrees_per_second = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum rate * 2^15. +*/ +long inv_get_gyro_sensitivity(void) +{ + return sensors.gyro.sensitivity; +} + +/** Accel sensitivity. +* @return A scale factor to convert device units to g's scaled by 2^16 +* such that g_s = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum accel value in g's * 2^15. +*/ +long inv_get_accel_sensitivity(void) +{ + return sensors.accel.sensitivity; +} + +/** Compass sensitivity. +* @return A scale factor to convert device units to micro Tesla scaled by 2^16 +* such that uT = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum uT * 2^15. +*/ +long inv_get_compass_sensitivity(void) +{ + return sensors.compass.sensitivity; +} + +/** Sets orientation and sensitivity field for a sensor. +* @param[out] sensor Structure to apply settings to +* @param[in] orientation Orientation description of how part is mounted. +* @param[in] sensitivity A Scale factor to convert from hardware units to +* standard units (dps, uT, g). +*/ +void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, + int orientation, long sensitivity) +{ + int error = 0; + + if (!sensitivity) { + // Sensitivity can't be zero + sensitivity = 1L<<16; + MPL_LOGE("\n\nCritical error! Sensitivity is zero.\n\n"); + } + + sensor->sensitivity = sensitivity; + // Make sure we don't describe some impossible orientation + if ((orientation & 3) == 3) { + error = 1; + } + if ((orientation & 0x18) == 0x18) { + error = 1; + } + if ((orientation & 0xc0) == 0xc0) { + error = 1; + } + if (error) { + orientation = 0x88; // Identity + MPL_LOGE("\n\nCritical error! Impossible mounting orientation given. Using Identity instead\n\n"); + } + sensor->orientation = orientation; +} + +/** 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() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 +* such that degrees_per_second = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum rate * 2^15. +*/ +void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_G_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.gyro, orientation, + sensitivity); +} + +/** Set Gyro Sample rate in micro seconds. +* @param[in] sample_rate_us Set Gyro Sample rate in us +*/ +void inv_set_gyro_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.gyro.sample_rate_us = sample_rate_us; + sensors.gyro.sample_rate_ms = sample_rate_us / 1000; + if (sensors.gyro.bandwidth == 0) { + sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +/** Set Accel Sample rate in micro seconds. +* @param[in] sample_rate_us Set Accel Sample rate in us +*/ +void inv_set_accel_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.accel.sample_rate_us = sample_rate_us; + sensors.accel.sample_rate_ms = sample_rate_us / 1000; + if (sensors.accel.bandwidth == 0) { + sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +/** Pick the smallest non-negative number. Priority to td1 on equal +* If both are negative, return the largest. +*/ +static int inv_pick_best_time_difference(long td1, long td2) +{ + if (td1 >= 0) { + if (td2 >= 0) { + if (td1 <= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } else { + // td1 + return 0; + } + } else if (td2 >= 0) { + // td2 + return 1; + } else { + // Both are negative + if (td1 >= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } +} + +/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data. +*/ +static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts) +{ + int status = 0; + switch (sensor_number) { + case 0: // Quat + *ts = sensors.quat.timestamp; + if (inv_data_builder.mode & INV_QUAT_NEW) + if (sensors.quat.timestamp_prev != sensors.quat.timestamp) + status = 1; + return status; + case 1: // Gyro + *ts = sensors.gyro.timestamp; + if (inv_data_builder.mode & INV_GYRO_NEW) + if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp) + status = 1; + return status; + case 2: // Compass + *ts = sensors.compass.timestamp; + if (inv_data_builder.mode & INV_MAG_NEW) + if (sensors.compass.timestamp_prev != sensors.compass.timestamp) + status = 1; + return status; + case 3: // Accel + *ts = sensors.accel.timestamp; + if (inv_data_builder.mode & INV_ACCEL_NEW) + if (sensors.accel.timestamp_prev != sensors.accel.timestamp) + status = 1; + return status; + default: + *ts = 0; + return 0; + } + return 0; +} + +/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. +* It does this by finding a raw sensor that has the closest sample rate that is at least as +* often desired. It also returns if that raw sensor has a new piece of data. +* Priority is 9-axis quat, 6-axis quat, 3-axis quat, gyro, compass, accel on ties. +* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. +*/ +int inv_get_9_axis_timestamp(long sample_rate_us, inv_time_t *ts) +{ + int status = 0; + long td[3]; + int idx,idx2; + + if ((sensors.quat.status & (INV_QUAT_9AXIS | INV_SENSOR_ON)) == (INV_QUAT_9AXIS | INV_SENSOR_ON)) { + // 9-axis from DMP + *ts = sensors.quat.timestamp; + if (inv_data_builder.mode & INV_QUAT_NEW) + if (sensors.quat.timestamp_prev != sensors.quat.timestamp) + status = 1; + return status; + } + + if ((sensors.compass.status & INV_SENSOR_ON) == 0) { + return 0; // Compass must be on + } + + // At this point, we know compass is on. Check if accel or 6-axis quat is on + td[0] = sample_rate_us - sensors.quat.sample_rate_us; + td[1] = sample_rate_us - sensors.compass.sample_rate_us; + if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { + // Sensor tied to compass or 6-axis + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; // Sensor number is 0 (Quat) or 2 (Compass) + return inv_raw_sensor_timestamp(idx, ts); + } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { + return 0; // Accel must be on or 6-axis quat must be on + } + + // At this point, we know accel is on. Check if 3-axis quat is on + td[2] = sample_rate_us - sensors.accel.sample_rate_us; + if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { + idx = inv_pick_best_time_difference(td[0], td[1]); + idx2 = inv_pick_best_time_difference(td[idx], td[2]); + if (idx2 == 1) + idx = 2; + if (idx > 0) + idx++; // Skip gyro sensor in next function call + // 0 = quat, 2 = compass, 3=accel + return inv_raw_sensor_timestamp(idx, ts); + } + + // No Quaternion data from outside, Gyro must be on + if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { + return 0; // Gyro must be on + } + + td[0] = sample_rate_us - sensors.gyro.sample_rate_us; + idx = inv_pick_best_time_difference(td[0], td[1]); + idx2 = inv_pick_best_time_difference(td[idx], td[2]); + if (idx2 == 1) + idx = 2; + idx++; + // 1 = gyro, 2 = compass, 3=accel + return inv_raw_sensor_timestamp(idx, ts); +} + +/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. +* It does this by finding a raw sensor that has the closest sample rate that is at least as +* often desired. It also returns if that raw sensor has a new piece of data. +* Priority compass, accel on a tie +* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. +*/ +int inv_get_6_axis_compass_accel_timestamp(long sample_rate_us, inv_time_t *ts) +{ + long td[2]; + int idx; + + if ((sensors.compass.status & INV_SENSOR_ON) == 0) { + return 0; // Compass must be on + } + if ((sensors.accel.status & INV_SENSOR_ON) == 0) { + return 0; // Accel must be on + } + + // At this point, we know compass & accel are both on. + td[0] = sample_rate_us - sensors.compass.sample_rate_us; + td[1] = sample_rate_us - sensors.accel.sample_rate_us; + idx = inv_pick_best_time_difference(td[0], td[1]); + idx += 2; + return inv_raw_sensor_timestamp(idx, ts); +} + +/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. +* It does this by finding a raw sensor that has the closest sample rate that is at least as +* often desired. It also returns if that raw sensor has a new piece of data. +* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel +* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. +*/ +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts) +{ + long td[2]; + int idx; + + if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { + // Sensor number is 0 (Quat) + return inv_raw_sensor_timestamp(0, ts); + } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { + return 0; // Accel must be on or 6-axis quat must be on + } + + // At this point, we know accel is on. Check if 3-axis quat is on + td[0] = sample_rate_us - sensors.quat.sample_rate_us; + td[1] = sample_rate_us - sensors.accel.sample_rate_us; + if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 3; + // 0 = quat, 3=accel + return inv_raw_sensor_timestamp(idx, ts); + } + + // No Quaternion data from outside, Gyro must be on + if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { + return 0; // Gyro must be on + } + + td[0] = sample_rate_us - sensors.gyro.sample_rate_us; + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; // 0=gyro 2=accel + idx++; + // 1 = gyro, 3=accel + return inv_raw_sensor_timestamp(idx, ts); +} + +/** Set Compass Sample rate in micro seconds. +* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. +*/ +void inv_set_compass_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.compass.sample_rate_us = sample_rate_us; + sensors.compass.sample_rate_ms = sample_rate_us / 1000; + if (sensors.compass.bandwidth == 0) { + 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 +*/ +void inv_set_quat_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.quat.sample_rate_us = sample_rate_us; + sensors.quat.sample_rate_ms = sample_rate_us / 1000; +} + +/** Set Gyro Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_gyro_bandwidth(int bandwidth_hz) +{ + sensors.gyro.bandwidth = bandwidth_hz; +} + +/** Set Accel Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_accel_bandwidth(int bandwidth_hz) +{ + sensors.accel.bandwidth = bandwidth_hz; +} + +/** Set Compass Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_compass_bandwidth(int bandwidth_hz) +{ + sensors.compass.bandwidth = bandwidth_hz; +} + +/** Helper function stating whether the compass is on or off. + * @return TRUE if compass if on, 0 if compass if off +*/ +int inv_get_compass_on() +{ + return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Helper function stating whether the gyro is on or off. + * @return TRUE if gyro if on, 0 if gyro if off +*/ +int inv_get_gyro_on() +{ + return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Helper function stating whether the acceleromter is on or off. + * @return TRUE if accel if on, 0 if accel if off +*/ +int inv_get_accel_on() +{ + return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Get last timestamp across all 3 sensors that are on. +* This find out which timestamp has the largest value for sensors that are on. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_time_t inv_get_last_timestamp() +{ + inv_time_t timestamp = 0; + if (sensors.accel.status & INV_SENSOR_ON) { + timestamp = sensors.accel.timestamp; + } + if (sensors.gyro.status & INV_SENSOR_ON) { + if (timestamp < sensors.gyro.timestamp) { + timestamp = sensors.gyro.timestamp; + } + MPL_LOGV("g ts: %lld", timestamp); + } + if (sensors.compass.status & INV_SENSOR_ON) { + if (timestamp < sensors.compass.timestamp) { + timestamp = sensors.compass.timestamp; + } + } + if (sensors.temp.status & INV_SENSOR_ON) { + if (timestamp < sensors.temp.timestamp) { + timestamp = sensors.temp.timestamp; + } + } + if (sensors.quat.status & INV_SENSOR_ON) { + if (timestamp < sensors.quat.timestamp) { + timestamp = sensors.quat.timestamp; + } + MPL_LOGV("q ts: %lld", timestamp); + } + + return timestamp; +} + +/** 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() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to g's +* such that g's = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum g_value * 2^15. +*/ +void inv_set_accel_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_A_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.accel, orientation, + sensitivity); +} + +/** 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() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to uT +* such that uT = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum uT_value * 2^15. +*/ +void inv_set_compass_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_C_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); +} + +void inv_matrix_vector_mult(const long *A, const long *x, long *y) +{ + y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); + y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); + y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); +} + +/** 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. +* @param[in,out] sensor structure to modify +* @param[in] bias bias in the mounting frame, in hardware units scaled by +* 2^16. Length 3. +*/ +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] -= bias[0] >> 1; + raw32[1] -= bias[1] >> 1; + raw32[2] -= bias[2] >> 1; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); + + sensor->status |= INV_CALIBRATED; +} + +/** Returns the current bias for the compass +* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. +* Length 3. +*/ +void inv_get_compass_bias(long *bias) +{ + if (bias != NULL) { + memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); + } +} + +/** Sets the compass bias +* @param[in] bias Length 3, in body frame, in hardware units scaled by 2^16 to allow fractional bit correction. +* @param[in] accuracy Accuracy of compass data, where 3=most accurate, and 0=least accurate. +*/ +void inv_set_compass_bias(const long *bias, int accuracy) +{ + if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { + memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); + 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); +} + +/** Set the state of a compass disturbance +* @param[in] dist 1=disturbance, 0=no disturbance +*/ +void inv_set_compass_disturbance(int dist) +{ + inv_data_builder.compass_disturbance = dist; +} + +int inv_get_compass_disturbance(void) { + return inv_data_builder.compass_disturbance; +} + +/** + * Sets the factory accel bias + * @param[in] bias + * Accel bias in hardware units (+/- 2 gee full scale assumed) + * scaled by 2^16. In chip mounting frame. Length of 3. + */ +void inv_set_accel_bias(const long *bias) +{ + if (!bias) + return; + + if (memcmp(inv_data_builder.save.factory_accel_bias, bias, + sizeof(inv_data_builder.save.factory_accel_bias))) { + memcpy(inv_data_builder.save.factory_accel_bias, bias, + sizeof(inv_data_builder.save.factory_accel_bias)); + } + inv_set_message(INV_MSG_NEW_FAB_EVENT, INV_MSG_NEW_FAB_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; +} + +/** Sets the accel bias with control over which axis. +* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame +* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. +* @param[in] mask Mask to select axis to apply bias set. +*/ +void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) +{ + if (bias) { + if (mask & 1){ + inv_data_builder.save_accel_mpl.accel_bias[0] = bias[0]; + } + if (mask & 2){ + inv_data_builder.save_accel_mpl.accel_bias[1] = bias[1]; + } + if (mask & 4){ + inv_data_builder.save_accel_mpl.accel_bias[2] = bias[2]; + } + + inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias); + } + inv_set_accel_accuracy(accuracy); + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + +#ifdef WIN32 +/** Sends out a message to activate writing 9-axis quaternion to the DMP. +*/ +void inv_overwrite_dmp_9quat(void) +{ + inv_set_message(INV_MSG_NEW_DMP_QUAT_WRITE_EVENT, INV_MSG_NEW_DMP_QUAT_WRITE_EVENT, 0); +} +#endif + +/** + * Sets the factory gyro bias + * @param[in] bias + * Gyro bias in hardware units (+/- 2000 dps full scale assumed) + * scaled by 2^16. In chip mounting frame. Length of 3. + */ +void inv_set_gyro_bias(const long *bias) +{ + if (!bias) + return; + + if (memcmp(inv_data_builder.save.factory_gyro_bias, bias, + sizeof(inv_data_builder.save.factory_gyro_bias))) { + memcpy(inv_data_builder.save.factory_gyro_bias, bias, + sizeof(inv_data_builder.save.factory_gyro_bias)); + } + inv_set_message(INV_MSG_NEW_FGB_EVENT, INV_MSG_NEW_FGB_EVENT, 0); +} + +/** + * Sets the mpl gyro bias + * @param[in] bias + * Gyro bias in hardware units scaled by 2^16 (+/- 2000 dps full + * scale assumed). In chip mounting frame. Length 3. + * @param[in] accuracy + * Accuracy of bias. 0 = least accurate, 3 = most accurate. + */ +void inv_set_mpl_gyro_bias(const long *bias, int accuracy) +{ + if (bias != NULL) { + if (memcmp(inv_data_builder.save_mpl.gyro_bias, bias, + sizeof(inv_data_builder.save_mpl.gyro_bias))) { + memcpy(inv_data_builder.save_mpl.gyro_bias, bias, + sizeof(inv_data_builder.save_mpl.gyro_bias)); + inv_apply_calibration(&sensors.gyro, + inv_data_builder.save_mpl.gyro_bias); + } + } + 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]; + else + /* Set to 27 deg C for now until we've got a better solution. */ + inv_data_builder.save.gyro_temp = 27L << 16; + inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); + + /* TODO: this flag works around the synchronization problem seen with using + the user-exposed message layer to signal the temperature compensation + module that gyro biases were set. + A better, cleaner method is certainly needed. */ + inv_data_builder.save.gyro_bias_tc_set = true; +} + +/** + * @internal + * @brief Return whether gyro biases were set to signal the temperature + * compensation algorithm that it can collect a data point to build + * the temperature slope while in no motion state. + * The flag clear automatically after is read. + * @return true if the flag was set, indicating gyro biases were set. + * false if the flag was not set. + */ +int inv_get_gyro_bias_tc_set(void) +{ + int flag = (inv_data_builder.save.gyro_bias_tc_set == true); + inv_data_builder.save.gyro_bias_tc_set = false; + return flag; +} + + +/** + * Get the mpl gyro biases + * @param[in] bias + * Gyro calibrated bias. + * Length 3. + */ +void inv_get_mpl_gyro_bias(long *bias, long *temp) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save_mpl.gyro_bias, + sizeof(inv_data_builder.save_mpl.gyro_bias)); + + if (temp != NULL) + temp[0] = inv_data_builder.save.gyro_temp; +} + +/** Gyro Bias in the form used by the DMP. +* @param[out] bias Gyro Bias in the form used by the DMP. It is scaled appropriately +* and is in the body frame as needed. If this bias is applied in the DMP +* then any quaternion must have the flag INV_BIAS_APPLIED set if it is a +* 3-axis quaternion, or INV_QUAT_6AXIS if it is a 6-axis quaternion +*/ +void inv_get_gyro_bias_dmp_units(long *bias) +{ + if (bias == NULL) + return; + inv_convert_to_body_with_scale(sensors.gyro.orientation, 46850825L, + inv_data_builder.save_mpl.gyro_bias, bias); +} + +/* TODO: Add this information to inv_sensor_cal_t */ +/** + * Get the gyro biases and temperature record from MPL + * @param[in] bias + * Gyro bias in hardware units scaled by 2^16. + * In chip mounting frame. + * Length 3. + */ +void inv_get_gyro_bias(long *bias) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save.factory_gyro_bias, + sizeof(inv_data_builder.save.factory_gyro_bias)); +} + +/** + * Get factory accel bias mask + * @param[in] bias + * Accel bias mask + * 1 is set, 0 is not set, Length 3 = x,y,z. + */ +int inv_get_factory_accel_bias_mask() +{ + long bias[3]; + int bias_mask = 0; + inv_get_accel_bias(bias); + if (bias != NULL) { + int i; + for(i = 0; i < 3; i++) { + if(bias[i] != 0) { + bias_mask |= 1 << i; + } else { + bias_mask &= ~ (1 << i); + } + } + } + return bias_mask; +} + +/** + * Get accel bias from MPL + * @param[in] bias + * Accel bias in hardware units scaled by 2^16. + * In chp mounting frame. + * Length 3. + */ +void inv_get_accel_bias(long *bias) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save.factory_accel_bias, + sizeof(inv_data_builder.save.factory_accel_bias)); +} + +/** Get Accel Bias +* @param[out] bias Accel bias +* @param[out] temp Temperature where 1 C = 2^16 +*/ +void inv_get_mpl_accel_bias(long *bias, long *temp) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save_accel_mpl.accel_bias, + sizeof(inv_data_builder.save_accel_mpl.accel_bias)); + if (temp != NULL) + temp[0] = inv_data_builder.save.accel_temp; +} + +/** Accel Bias in the form used by the DMP. +* @param[out] bias Accel Bias in the form used by the DMP. It is scaled appropriately +* and is in the body frame as needed. +*/ +void inv_get_accel_bias_dmp_units(long *bias) +{ + if (bias == NULL) + return; + inv_convert_to_body_with_scale(sensors.accel.orientation, 536870912L, + inv_data_builder.save_accel_mpl.accel_bias, bias); +} + +/** + * Record new accel data for use when inv_execute_on_data() is called + * @param[in] accel + * accel data, length 3. + * Calibrated data is in m/s^2 scaled by 2^16 in body frame. + * Raw data is in device units in chip mounting frame. + * @param[in] status + * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 + * being most accurate. + * The upper bit INV_CALIBRATED, is set if the data was calibrated + * outside MPL and it is not set if the data being passed is raw. + * Raw data should be in device units, typically in a 16-bit range. + * @param[in] timestamp + * Monotonic time stamp, for Android it's in nanoseconds. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_ACCEL; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + if ((status & INV_CALIBRATED) == 0) { + sensors.accel.raw[0] = (short)accel[0]; + sensors.accel.raw[1] = (short)accel[1]; + sensors.accel.raw[2] = (short)accel[2]; + sensors.accel.status |= INV_RAW_DATA; + inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias); + } else { + sensors.accel.calibrated[0] = accel[0]; + sensors.accel.calibrated[1] = accel[1]; + 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; + sensors.accel.timestamp = timestamp; + + return INV_SUCCESS; +} + +/** Record new gyro data and calls inv_execute_on_data() if previous +* sample has not been processed. +* @param[in] gyro Data is in device units. Length 3. +* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_GYRO; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); + sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.gyro.timestamp_prev = sensors.gyro.timestamp; + sensors.gyro.timestamp = timestamp; + inv_apply_calibration(&sensors.gyro, inv_data_builder.save_mpl.gyro_bias); + + return INV_SUCCESS; +} + +/** Record new compass data for use when inv_execute_on_data() is called +* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. +* Length 3. +* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. +* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is +* not set if the data being passed is raw. Raw data should be in device units, typically +* in a 16-bit range. +* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_compass(const long *compass, int status, + inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_COMPASS; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + if ((status & INV_CALIBRATED) == 0) { + long data[3]; + inv_set_compass_soft_iron_input_data(compass); + inv_get_compass_soft_iron_output_data(data); + sensors.compass.raw[0] = (short)data[0]; + sensors.compass.raw[1] = (short)data[1]; + sensors.compass.raw[2] = (short)data[2]; + inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); + sensors.compass.status |= INV_RAW_DATA; + } else { + sensors.compass.calibrated[0] = compass[0]; + sensors.compass.calibrated[1] = compass[1]; + 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; + sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; + + return INV_SUCCESS; +} + +/** Record new temperature data for use when inv_execute_on_data() is called. + * @param[in] temp Temperature data in q16 format. + * @param[in] timestamp Monotonic time stamp; for Android it's in + * nanoseconds. +* @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_TEMPERATURE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + sensors.temp.calibrated[0] = temp; + sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.temp.timestamp_prev = sensors.temp.timestamp; + sensors.temp.timestamp = timestamp; + /* TODO: Apply scale, remove offset. */ + + return INV_SUCCESS; +} +/** quaternion data +* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. +* Real part first. Length 4. +* @param[in] status number of axis, 16-bit or 32-bit +* set INV_QUAT_3ELEMENT if input quaternion has only 3 elements (no scalar). +* inv_compute_scalar_part() assumes 32-bit data. If using 16-bit quaternion, +* shift 16 bits first before calling this function. +* @param[in] timestamp +* @param[in] timestamp Monotonic time stamp; for Android it's in +* nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_QUAT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + /* Android version DMP does not have scalar part */ + if (status & INV_QUAT_3ELEMENT) { + long resultQuat[4]; + MPL_LOGV("3q: %ld,%ld,%ld\n", quat[0], quat[1], quat[2]); + inv_compute_scalar_part(quat, resultQuat); + MPL_LOGV("4q: %ld,%ld,%ld,%ld\n", resultQuat[0], resultQuat[1], resultQuat[2], resultQuat[3]); + memcpy(sensors.quat.raw, resultQuat, sizeof(sensors.quat.raw)); + } else { + memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); + } + sensors.quat.timestamp_prev = sensors.quat.timestamp; + sensors.quat.timestamp = timestamp; + sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.quat.status |= (INV_QUAT_3AXIS & status); + sensors.quat.status |= (INV_QUAT_6AXIS & status); + sensors.quat.status |= (INV_QUAT_9AXIS & status); + sensors.quat.status |= (INV_BIAS_APPLIED & status); + sensors.quat.status |= (INV_DMP_BIAS_APPLIED & status); + sensors.quat.status |= (INV_QUAT_3ELEMENT & status); + MPL_LOGV("quat.status: %d", sensors.quat.status); + if (sensors.quat.status & (INV_QUAT_9AXIS | INV_QUAT_6AXIS)) { + // Set quaternion + inv_store_gaming_quaternion(quat, timestamp); + } + if (sensors.quat.status & INV_QUAT_9AXIS) { + long identity[4] = {(1L<<30), 0, 0, 0}; + inv_set_compass_correction(identity, timestamp); + } + return INV_SUCCESS; +} + +inv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp) +{ + sensors.pressure.status |= INV_NEW_DATA; + return INV_SUCCESS; +} + +/** This should be called when the accel has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_accel_was_turned_off() +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_COMPASS_OFF; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + sensors.accel.status = 0; +} + +/** This should be called when the compass has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_compass_was_turned_off() +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_COMPASS_OFF; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + sensors.compass.status = 0; +} + +/** This should be called when the quaternion data from the DMP has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_quaternion_sensor_was_turned_off(void) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_QUAT_OFF; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + sensors.quat.status = 0; +} + +/** This should be called when the gyro has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_gyro_was_turned_off() +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_GYRO_OFF; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + sensors.gyro.status = 0; +} + +/** This should be called when the temperature sensor has been turned off. + * This is so that we will know if the data is contiguous. + */ +void inv_temperature_was_turned_off() +{ + sensors.temp.status = 0; +} + +/** Registers to receive a callback when there is new sensor data. +* @internal +* @param[in] func Function pointer to receive callback when there is new sensor data +* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority +* numbers must be unique. +* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be +* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = +* gyro data, INV_MAG_NEW = compass data. So passing in +* INV_ACCEL_NEW | INV_MAG_NEW, a +* callback would be generated if there was new magnetomer data OR new accel data. +*/ +inv_error_t inv_register_data_cb( + inv_error_t (*func)(struct inv_sensor_cal_t *data), + int priority, int sensor_type) +{ + inv_error_t result = INV_SUCCESS; + int kk, nn; + + // Make sure we haven't registered this function already + // Or used the same priority + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if ((inv_data_builder.process[kk].func == func) || + (inv_data_builder.process[kk].priority == priority)) { + return INV_ERROR_INVALID_PARAMETER; //fixme give a warning + } + } + + // Make sure we have not filled up our number of allowable callbacks + if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { + kk = 0; + if (inv_data_builder.num_cb != 0) { + // set kk to be where this new callback goes in the array + while ((kk < inv_data_builder.num_cb) && + (inv_data_builder.process[kk].priority < priority)) { + kk++; + } + if (kk != inv_data_builder.num_cb) { + // We need to move the others + for (nn = inv_data_builder.num_cb; nn > kk; --nn) { + inv_data_builder.process[nn] = + inv_data_builder.process[nn - 1]; + } + } + } + // Add new callback + inv_data_builder.process[kk].func = func; + inv_data_builder.process[kk].priority = priority; + inv_data_builder.process[kk].data_required = sensor_type; + inv_data_builder.num_cb++; + } else { + MPL_LOGE("Unable to add feature callback as too many were already registered\n"); + result = INV_ERROR_MEMORY_EXAUSTED; + } + + return result; +} + +/** Unregisters the callback that happens when new sensor data is received. +* @internal +* @param[in] func Function pointer to receive callback when there is new sensor data +* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority +* numbers must be unique. +* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be +* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = +* gyro data, INV_MAG_NEW = compass data. So passing in +* INV_ACCEL_NEW | INV_MAG_NEW, a +* callback would be generated if there was new magnetomer data OR new accel data. +*/ +inv_error_t inv_unregister_data_cb( + inv_error_t (*func)(struct inv_sensor_cal_t *data)) +{ + int kk, nn; + + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if (inv_data_builder.process[kk].func == func) { + // Delete this callback + for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { + inv_data_builder.process[nn - 1] = + inv_data_builder.process[nn]; + } + inv_data_builder.num_cb--; + return INV_SUCCESS; + } + } + + return INV_SUCCESS; // We did not find the callback +} + +/** After at least one of inv_build_gyro(), inv_build_accel(), or +* inv_build_compass() has been called, this function should be called. +* It will process the data it has received and update all the internal states +* and features that have been turned on. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_execute_on_data(void) +{ + inv_error_t result, first_error; + int kk; + +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_EXECUTE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + // Determine what new data we have + inv_data_builder.mode = 0; + if (sensors.gyro.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_GYRO_NEW; + if (sensors.accel.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_ACCEL_NEW; + if (sensors.compass.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_MAG_NEW; + if (sensors.temp.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_TEMP_NEW; + if (sensors.quat.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_QUAT_NEW; + if (sensors.pressure.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_PRESSURE_NEW; + + first_error = INV_SUCCESS; + + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) { + result = inv_data_builder.process[kk].func(&sensors); + if (result && !first_error) { + first_error = result; + } + } + } + + inv_set_contiguous(); + + return first_error; +} + +/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. +* +*/ +static void inv_set_contiguous(void) +{ + inv_time_t current_time = 0; + if (sensors.gyro.status & INV_NEW_DATA) { + sensors.gyro.status |= INV_CONTIGUOUS; + current_time = sensors.gyro.timestamp; + } + if (sensors.accel.status & INV_NEW_DATA) { + sensors.accel.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.accel.timestamp); + } + if (sensors.compass.status & INV_NEW_DATA) { + sensors.compass.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.compass.timestamp); + } + if (sensors.temp.status & INV_NEW_DATA) { + sensors.temp.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.temp.timestamp); + } + if (sensors.quat.status & INV_NEW_DATA) { + sensors.quat.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.quat.timestamp); + } + +#if 0 + /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() + * type functions. This is just in case that breaks down. We make sure + * all the data is within 2 seconds of the newest piece of data*/ + if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) + inv_gyro_was_turned_off(); + if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) + inv_accel_was_turned_off(); + if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) + inv_compass_was_turned_off(); + /* TODO: Temperature might not need to be read this quickly. */ + if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) + inv_temperature_was_turned_off(); +#endif + + /* clear bits */ + sensors.gyro.status &= ~INV_NEW_DATA; + sensors.accel.status &= ~INV_NEW_DATA; + sensors.compass.status &= ~INV_NEW_DATA; + sensors.temp.status &= ~INV_NEW_DATA; + sensors.quat.status &= ~INV_NEW_DATA; + sensors.pressure.status &= ~INV_NEW_DATA; +} + +/** Gets a whole set of accel data including data, accuracy and timestamp. + * @param[out] data Accel Data where 1g = 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_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + if (data != NULL) { + memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); + } + if (timestamp != NULL) { + *timestamp = sensors.accel.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.accel.accuracy; + } +} + +/** Gets a whole set of gyro 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(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); + if (timestamp != NULL) { + *timestamp = sensors.gyro.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.gyro.accuracy; + } +} + +/** 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 = 0; + } +} + +/** Get's latest gyro data. +* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. +*/ +void inv_get_gyro(long *gyro) +{ + memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); +} + +/** Gets a whole set of compass data including data, accuracy and timestamp. + * @param[out] data Compass Data where 1 uT = 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_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); + if (timestamp != NULL) { + *timestamp = sensors.compass.timestamp; + } + if (accuracy != NULL) { + if (inv_data_builder.compass_disturbance) + *accuracy = 0; + else + *accuracy = sensors.compass.accuracy; + } +} + +/** Gets a whole set of compass raw data including data, accuracy and timestamp. + * @param[out] data Compass Data where 1 uT = 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_compass_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.compass.raw_scaled, sizeof(sensors.compass.raw_scaled)); + if (timestamp != NULL) { + *timestamp = sensors.compass.timestamp; + } + //per Michele, since data is raw, accuracy should = 0 + *accuracy = 0; +} + +/** Gets a whole set of temperature data including data, accuracy and timestamp. + * @param[out] data Temperature data where 1 degree C = 2^16 + * @param[out] accuracy 0 to 3, where 3 is most accurate. + * @param[out] timestamp The timestamp of the data sample. + */ +void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) +{ + data[0] = sensors.temp.calibrated[0]; + if (timestamp) + *timestamp = sensors.temp.timestamp; + if (accuracy) + *accuracy = sensors.temp.accuracy; +} + +/** Returns accuracy of gyro. + * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. +*/ +int inv_get_gyro_accuracy(void) +{ + return sensors.gyro.accuracy; +} + +/** Returns accuracy of compass. + * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. +*/ +int inv_get_mag_accuracy(void) +{ + if (inv_data_builder.compass_disturbance) + return 0; + return sensors.compass.accuracy; +} + +/** Returns accuracy of accel. + * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. +*/ +int inv_get_accel_accuracy(void) +{ + return sensors.accel.accuracy; +} + +inv_error_t inv_get_gyro_orient(int *orient) +{ + *orient = sensors.gyro.orientation; + return 0; +} + +inv_error_t inv_get_accel_orient(int *orient) +{ + *orient = sensors.accel.orientation; + return 0; +} + +/*======================================================================*/ +/* compass soft iron module */ +/*======================================================================*/ + +/** Gets the 3x3 compass transform matrix in 32 bit Q30 fixed point format. + * @param[out] the pointer of the 3x3 matrix in Q30 format +*/ +void inv_get_compass_soft_iron_matrix_d(long *matrix) { + int i; + for (i=0; i<9; i++) { + matrix[i] = sensors.soft_iron.matrix_d[i]; + } +} + +/** Sets the 3x3 compass transform matrix in 32 bit Q30 fixed point format. + * @param[in] the pointer of the 3x3 matrix in Q30 format +*/ +void inv_set_compass_soft_iron_matrix_d(long *matrix) { + int i; + for (i=0; i<9; i++) { + // set the floating point matrix + sensors.soft_iron.matrix_d[i] = matrix[i]; + // convert to Q30 format + sensors.soft_iron.matrix_f[i] = inv_q30_to_float(matrix[i]); + } +} +/** Gets the 3x3 compass transform matrix in 32 bit floating point format. + * @param[out] the pointer of the 3x3 matrix in floating point format +*/ +void inv_get_compass_soft_iron_matrix_f(float *matrix) { + int i; + for (i=0; i<9; i++) { + matrix[i] = sensors.soft_iron.matrix_f[i]; + } +} +/** Sets the 3x3 compass transform matrix in 32 bit floating point format. + * @param[in] the pointer of the 3x3 matrix in floating point format +*/ +void inv_set_compass_soft_iron_matrix_f(float *matrix) { + int i; + for (i=0; i<9; i++) { + // set the floating point matrix + sensors.soft_iron.matrix_f[i] = matrix[i]; + // convert to Q30 format + sensors.soft_iron.matrix_d[i] = (long )(matrix[i]*ROT_MATRIX_SCALE_LONG); + } +} + +/** This subroutine gets the fixed point Q30 compass data after the soft iron transformation. + * @param[out] the pointer of the 3x1 vector compass data in MPL format +*/ +void inv_get_compass_soft_iron_output_data(long *data) { + int i; + for (i=0; i<3; i++) { + data[i] = sensors.soft_iron.trans[i]; + } +} +/** This subroutine gets the fixed point Q30 compass data before the soft iron transformation. + * @param[out] the pointer of the 3x1 vector compass data in MPL format +*/ +void inv_get_compass_soft_iron_input_data(long *data) { + int i; + for (i=0; i<3; i++) { + data[i] = sensors.soft_iron.raw[i]; + } +} +/** This subroutine sets the compass raw data for the soft iron transformation. + * @param[int] the pointer of the 3x1 vector compass raw data in MPL format +*/ +void inv_set_compass_soft_iron_input_data(const long *data) { + int i; + for (i=0; i<3; i++) { + sensors.soft_iron.raw[i] = data[i]; + } + if (sensors.soft_iron.enable == 1) { + mlMatrixVectorMult(sensors.soft_iron.matrix_d, data, sensors.soft_iron.trans); + } else { + for (i=0; i<3; i++) { + sensors.soft_iron.trans[i] = data[i]; + } + } +} + +/** This subroutine resets the the soft iron transformation to unity matrix and + * disable the soft iron transformation process by default. +*/ +void inv_reset_compass_soft_iron_matrix(void) { + int i; + for (i=0; i<9; i++) { + sensors.soft_iron.matrix_f[i] = 0.0f; + } + + memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d)); + + for (i=0; i<3; i++) { + // set the floating point matrix + sensors.soft_iron.matrix_f[i*4] = 1.0; + // set the fixed point matrix + sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG; + } + + inv_disable_compass_soft_iron_matrix(); +} + +/** This subroutine enables the the soft iron transformation process. +*/ +void inv_enable_compass_soft_iron_matrix(void) { + sensors.soft_iron.enable = 1; +} + +/** This subroutine disables the the soft iron transformation process. +*/ +void inv_disable_compass_soft_iron_matrix(void) { + sensors.soft_iron.enable = 0; +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/data_builder.h b/6515/libsensors_iio/software/core/mllite/data_builder.h new file mode 100644 index 0000000..542f76b --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/data_builder.h @@ -0,0 +1,333 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_DATA_BUILDER_H__ +#define INV_DATA_BUILDER_H__ + +#include <stdio.h> +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// Uncomment this flag to enable playback debug and record or playback scenarios +//#define INV_PLAYBACK_DBG + +/** This is a new sample of accel data */ +#define INV_ACCEL_NEW 1 +/** This is a new sample of gyro data */ +#define INV_GYRO_NEW 2 +/** This is a new sample of compass data */ +#define INV_MAG_NEW 4 +/** This is a new sample of temperature data */ +#define INV_TEMP_NEW 8 +/** This is a new sample of quaternion data */ +#define INV_QUAT_NEW 16 +/** This is a new sample of pressure data */ +#define INV_PRESSURE_NEW 32 + +/** Set if the data is contiguous. Typically not set if a sample was skipped */ +#define INV_CONTIGUOUS 16 +/** Set if the calibrated data has been solved for */ +#define INV_CALIBRATED 32 +/** INV_NEW_DATA set for a new set of data, cleared if not available. */ +#define INV_NEW_DATA 64 +/** Set if raw data exists */ +#define INV_RAW_DATA 128 +/** Set if the sensor is on */ +#define INV_SENSOR_ON 256 +/** Set if quaternion has bias correction applied */ +#define INV_BIAS_APPLIED 512 +/** Set if quaternion is 6-axis from DMP */ +#define INV_QUAT_6AXIS 1024 +/** Set if quaternion is 9 axis from DMP */ +#define INV_QUAT_9AXIS 2048 +/** Set if quaternion is 3-axis from DMP */ +#define INV_QUAT_3AXIS 4096 +/** Set if DMP has applied bias */ +#define INV_DMP_BIAS_APPLIED 8192 +/** Set if quaternion is 3 elements (android only) */ +#define INV_QUAT_3ELEMENT 16384 + +#define INV_PRIORITY_MOTION_NO_MOTION 100 +#define INV_PRIORITY_GYRO_TC 150 +#define INV_PRIORITY_QUATERNION_GYRO_ACCEL 200 +#define INV_PRIORITY_QUATERNION_NO_GYRO 250 +#define INV_PRIORITY_MAGNETIC_DISTURBANCE 300 +#define INV_PRIORITY_HEADING_FROM_GYRO 350 +#define INV_PRIORITY_COMPASS_BIAS_W_GYRO 375 +#define INV_PRIORITY_COMPASS_VECTOR_CAL 400 +#define INV_PRIORITY_COMPASS_ADV_BIAS 500 +#define INV_PRIORITY_9_AXIS_FUSION 600 +#define INV_PRIORITY_9_AXIS_FUSION_LIGHT 650 +#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS 700 +#define INV_PRIORITY_QUATERNION_ACCURACY 750 +#define INV_PRIORITY_RESULTS_HOLDER 800 +#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 { + /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when + * the rotation matrix could be thought of only having elements of 0,1,-1. + * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign. + * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row. + * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row. + * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row. + */ + 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; + /** Sample rate in microseconds */ + long sample_rate_us; + long sample_rate_ms; + /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample + * skipped due to power savings turning off this sensor. + * INV_NEW_DATA set for a new set of data, cleared if not available. + * INV_CALIBRATED_SET if calibrated data has been solved for */ + int status; + /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */ + int accuracy; + inv_time_t timestamp; + inv_time_t timestamp_prev; + /** Bandwidth in Hz */ + int bandwidth; +}; + +struct inv_quat_sensor_t { + long raw[4]; + /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample + * skipped due to power savings turning off this sensor. + * INV_NEW_DATA set for a new set of data, cleared if not available. + * INV_CALIBRATED_SET if calibrated data has been solved for */ + int status; + inv_time_t timestamp; + inv_time_t timestamp_prev; + long sample_rate_us; + long sample_rate_ms; +}; + +struct inv_soft_iron_t { + long raw[3]; + long trans[3]; + long matrix_d[9]; // Q30 format fixed point. The dynamic range is (-2.0 to 2.0); + float matrix_f[9]; + + int enable; +}; + +struct inv_sensor_cal_t { + struct inv_single_sensor_t gyro; + struct inv_single_sensor_t accel; + struct inv_single_sensor_t compass; + struct inv_single_sensor_t temp; + struct inv_quat_sensor_t quat; + struct inv_single_sensor_t pressure; + struct inv_soft_iron_t soft_iron; + /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate + * which data is a new sample as these data points may have different sample rates. + */ + int status; +}; + +// Useful for debug record and playback +typedef enum { + RD_NO_DEBUG, + RD_RECORD, + RD_PLAYBACK +} rd_dbg_mode; + +typedef enum { + PLAYBACK_DBG_TYPE_GYRO, + PLAYBACK_DBG_TYPE_ACCEL, + PLAYBACK_DBG_TYPE_COMPASS, + PLAYBACK_DBG_TYPE_TEMPERATURE, + PLAYBACK_DBG_TYPE_EXECUTE, + PLAYBACK_DBG_TYPE_A_ORIENT, + PLAYBACK_DBG_TYPE_G_ORIENT, + PLAYBACK_DBG_TYPE_C_ORIENT, + PLAYBACK_DBG_TYPE_A_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_C_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_G_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_GYRO_OFF, + PLAYBACK_DBG_TYPE_ACCEL_OFF, + PLAYBACK_DBG_TYPE_COMPASS_OFF, + PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_QUAT, + PLAYBACK_DBG_TYPE_QUAT_OFF +} inv_rd_dbg_states; + +/** Change this key if the definition of the struct inv_db_save_t changes. + Previous keys: 53394, 53395, 53396 */ +#define INV_DB_SAVE_KEY (53397) + +#define INV_DB_SAVE_MPL_KEY (50001) +#define INV_DB_SAVE_ACCEL_MPL_KEY (50002) + +struct inv_db_save_t { + /** compass Bias in chip frame, hardware units scaled by 2^16. */ + long compass_bias[3]; + /** gyro factory bias in chip frame, hardware units scaled by 2^16, + +/- 2000 dps full scale. */ + long factory_gyro_bias[3]; + /** accel factory bias in chip frame, hardware units scaled by 2^16, + +/- 2 gee full scale. */ + long factory_accel_bias[3]; + /** temperature when factory_gyro_bias was stored. */ + long gyro_temp; + /** flag to indicate temperature compensation that biases where stored. */ + int gyro_bias_tc_set; + /** temperature when accel bias was stored. */ + long accel_temp; + long gyro_temp_slope[3]; + /** sensor accuracies */ + int gyro_accuracy; + int accel_accuracy; + int compass_accuracy; +}; + +struct inv_db_save_mpl_t { + /** gyro bias in chip frame, hardware units scaled by 2^16, +/- 2000 dps + full scale */ + long gyro_bias[3]; +}; + +struct inv_db_save_accel_mpl_t { + /** accel bias in chip frame, hardware units scaled by 2^16, +/- 2 gee + full scale */ + long accel_bias[3]; +}; + +/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/ +#define INV_MAX_DATA_CB 20 + +#ifdef INV_PLAYBACK_DBG +void inv_turn_on_data_logging(FILE *file); +void inv_turn_off_data_logging(); +#endif + +void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity); +void inv_set_accel_orientation_and_scale(int orientation, + long sensitivity); +void inv_set_compass_orientation_and_scale(int orientation, + long sensitivity); +void inv_set_gyro_sample_rate(long sample_rate_us); +void inv_set_compass_sample_rate(long sample_rate_us); +void inv_set_quat_sample_rate(long sample_rate_us); +void inv_set_accel_sample_rate(long sample_rate_us); +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); +inv_error_t inv_unregister_data_cb(inv_error_t (*func) + (struct inv_sensor_cal_t * data)); + +inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp); +inv_error_t inv_build_compass(const long *compass, int status, + inv_time_t timestamp); +inv_error_t inv_build_accel(const long *accel, int status, + inv_time_t timestamp); +inv_error_t inv_build_temp(const long temp, inv_time_t timestamp); +inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp); +inv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp); +inv_error_t inv_execute_on_data(void); + +void inv_get_compass_bias(long *bias); + +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); +void inv_set_mpl_gyro_bias(const long *bias, int accuracy); +void inv_set_accel_bias(const long *bias); +void inv_set_mpl_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_compass_soft_iron_matrix_d(long *matrix); +void inv_set_compass_soft_iron_matrix_d(long *matrix); + +void inv_get_compass_soft_iron_matrix_f(float *matrix); +void inv_set_compass_soft_iron_matrix_f(float *matrix); + +void inv_get_compass_soft_iron_output_data(long *data); +void inv_get_compass_soft_iron_input_data(long *data); +void inv_set_compass_soft_iron_input_data(const long *data); + +void inv_reset_compass_soft_iron_matrix(void); +void inv_enable_compass_soft_iron_matrix(void); +void inv_disable_compass_soft_iron_matrix(void); + +void inv_get_mpl_gyro_bias(long *bias, long *temp); +void inv_get_gyro_bias(long *bias); +void inv_get_gyro_bias_dmp_units(long *bias); +int inv_get_factory_accel_bias_mask(); +void inv_get_mpl_accel_bias(long *bias, long *temp); +void inv_get_accel_bias(long *bias); +void inv_get_accel_bias_dmp_units(long *bias); + +void inv_gyro_was_turned_off(void); +void inv_accel_was_turned_off(void); +void inv_compass_was_turned_off(void); +void inv_quaternion_sensor_was_turned_off(void); +inv_error_t inv_init_data_builder(void); +long inv_get_gyro_sensitivity(void); +long inv_get_accel_sensitivity(void); +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_compass_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); + +void inv_get_gyro(long *gyro); + +int inv_get_gyro_accuracy(void); +int inv_get_accel_accuracy(void); +int inv_get_mag_accuracy(void); +void inv_get_raw_compass(short *raw); + +int inv_get_compass_on(void); +int inv_get_gyro_on(void); +int inv_get_accel_on(void); + +inv_time_t inv_get_last_timestamp(void); +int inv_get_compass_disturbance(void); + +// new DMP cal functions +inv_error_t inv_get_gyro_orient(int *orient); +inv_error_t inv_get_accel_orient(int *orient); + +#ifdef WIN32 +void inv_overwrite_dmp_9quat(void); +#endif + +// internal +int inv_get_gyro_bias_tc_set(void); +int inv_get_9_axis_timestamp(long sample_rate_us, inv_time_t *ts); +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts); +int inv_get_6_axis_compass_accel_timestamp(long sample_rate_us, inv_time_t *ts); + +#ifdef __cplusplus +} +#endif + +#endif /* INV_DATA_BUILDER_H__ */ diff --git a/6515/libsensors_iio/software/core/mllite/hal_outputs.c b/6515/libsensors_iio/software/core/mllite/hal_outputs.c new file mode 100644 index 0000000..1e3bb72 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/hal_outputs.c @@ -0,0 +1,772 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + 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 + * @brief HAL Outputs. + */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 1 /* Use 0 to turn on MPL_LOGV output */ +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MLLITE" +//#define MPL_LOG_9AXIS_DEBUG 1 + +#include <string.h> + +#include "hal_outputs.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" +#include "start_manager.h" +#include "data_builder.h" +#include "results_holder.h" + +/* commenting this define out will bypass the low pass filter noise reduction + filter for compass data. + Disable this only for testing purpose (e.g. comparing the raw and calibrated + compass data, since the former is unfiltered and the latter is filtered, + leading to a small difference in the readings sample by sample). + Android specifications require this filter to be enabled to have the Magnetic + Field output's standard deviation fall below 0.5 uT. + */ +#define CALIB_COMPASS_NOISE_REDUCTION + +struct hal_output_t { + int accuracy_mag; /**< Compass accuracy */ + //int accuracy_gyro; /**< Gyro Accuracy */ + //int accuracy_accel; /**< Accel Accuracy */ + int accuracy_quat; /**< quat Accuracy */ + + inv_time_t nav_timestamp; + inv_time_t gam_timestamp; + //inv_time_t accel_timestamp; + inv_time_t mag_timestamp; + long nav_quat[4]; + int gyro_status; + int accel_status; + int compass_status; + int nine_axis_status; + int quat_status; + inv_biquad_filter_t lp_filter[3]; + float compass_float[3]; + long linear_acceleration_sample_rate_us; + long orientation_sample_rate_us; + long rotation_vector_sample_rate_us; + long gravity_sample_rate_us; + long orientation_6_axis_sample_rate_us; + long orientation_geomagnetic_sample_rate_us; + long rotation_vector_6_axis_sample_rate_us; + long geomagnetic_rotation_vector_sample_rate_us; +}; + +static struct hal_output_t hal_out; + +void inv_set_linear_acceleration_sample_rate(long sample_rate_us) +{ + hal_out.linear_acceleration_sample_rate_us = sample_rate_us; +} + +void inv_set_orientation_sample_rate(long sample_rate_us) +{ + hal_out.orientation_sample_rate_us = sample_rate_us; +} + +void inv_set_rotation_vector_sample_rate(long sample_rate_us) +{ + hal_out.rotation_vector_sample_rate_us = sample_rate_us; +} + +void inv_set_gravity_sample_rate(long sample_rate_us) +{ + hal_out.gravity_sample_rate_us = sample_rate_us; +} + +void inv_set_orientation_6_axis_sample_rate(long sample_rate_us) +{ + hal_out.orientation_6_axis_sample_rate_us = sample_rate_us; +} + +void inv_set_orientation_geomagnetic_sample_rate(long sample_rate_us) +{ + hal_out.geomagnetic_rotation_vector_sample_rate_us = sample_rate_us; +} + +void inv_set_rotation_vector_6_axis_sample_rate(long sample_rate_us) +{ + hal_out.rotation_vector_6_axis_sample_rate_us = sample_rate_us; +} + +void inv_set_geomagnetic_rotation_vector_sample_rate(long sample_rate_us) +{ + hal_out.geomagnetic_rotation_vector_sample_rate_us = sample_rate_us; +} + +/** Acceleration (m/s^2) in body frame. +* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it +* should return a vector of magnitude near 9.81 m/s^2 +* @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_accel(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + int status; + /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. + * So this 9.80665 / 2^16 */ +#define ACCEL_CONVERSION 0.000149637603759766f + long accel[3]; + inv_get_accel_set(accel, accuracy, timestamp); + values[0] = accel[0] * ACCEL_CONVERSION; + values[1] = accel[1] * ACCEL_CONVERSION; + values[2] = accel[2] * ACCEL_CONVERSION; + if (hal_out.accel_status & INV_NEW_DATA) + status = 1; + else + status = 0; + MPL_LOGV("accel values:%f %f %f -%d -%lld", values[0], values[1], + values[2], status, *timestamp); + return status; +} + +/** Linear Acceleration (m/s^2) in Body Frame. +* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show +* accel biases while at rest. +* @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_accel(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gravity[3], accel[3]; + inv_time_t timestamp1; + + inv_get_accel_set(accel, accuracy, ×tamp1); + inv_get_gravity(gravity); + accel[0] -= gravity[0] >> 14; + accel[1] -= gravity[1] >> 14; + accel[2] -= gravity[2] >> 14; + values[0] = accel[0] * ACCEL_CONVERSION; + values[1] = accel[1] * ACCEL_CONVERSION; + values[2] = accel[2] * ACCEL_CONVERSION; + + return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp); +} + +/** Gravity vector (m/s^2) in Body Frame. +* @param[out] values Gravity vector in body frame, length 3, (m/s^2) +* @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_accel(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gravity[3]; + + *accuracy = (int8_t) hal_out.accuracy_quat; + inv_get_gravity(gravity); + values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; + values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; + values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; + + return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, 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 + +/** Gyroscope calibrated data (rad/s) in body frame. +* @param[out] values Rotation Rate in rad/sec. +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @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(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gyro[3]; + int status; + + inv_get_gyro_set(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; +} + +/** 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 +* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ +* around an axis {x, y, z}. <br> +* The three elements of the rotation vector are +* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation +* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is +* equal to the direction of the axis of rotation. +* +* The three elements of the rotation vector are equal to the last three components of a unit quaternion +* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2). +* +* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. +* The reference coordinate system is defined as a direct orthonormal basis, where: +* +* -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). +* -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. +* -Z points towards the sky and is perpendicular to the ground. +* @param[out] values +* Length 5, 4th element being the w angle of the originating 4 +* elements quaternion and 5th element being the heading accuracy +* at 95%. +* @param[out] accuracy Accuracy is not defined +* @param[out] timestamp Timestamp. In (ns) for Android. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + float quat_float[4]; + *accuracy = (int8_t) hal_out.accuracy_quat; + inv_get_quaternion_float(quat_float); + + if (quat_float[0] >= .0) { + values[0] = quat_float[1]; + values[1] = quat_float[2]; + values[2] = quat_float[3]; + values[3] = quat_float[0]; + } else { + values[0] = -quat_float[1]; + values[1] = -quat_float[2]; + values[2] = -quat_float[3]; + values[3] = -quat_float[0]; + } + values[4] = inv_get_heading_confidence_interval(); + return inv_get_9_axis_timestamp(hal_out.rotation_vector_sample_rate_us, timestamp); +} + +int inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + int status; + long accel[3]; + float quat_6_axis[4]; + inv_time_t timestamp1; + inv_get_accel_set(accel, accuracy, ×tamp1); + inv_get_6axis_quaternion_float(quat_6_axis, ×tamp1); + + if (quat_6_axis[0] >= .0) { + values[0] = quat_6_axis[1]; + values[1] = quat_6_axis[2]; + values[2] = quat_6_axis[3]; + values[3] = quat_6_axis[0]; + } else { + values[0] = -quat_6_axis[1]; + values[1] = -quat_6_axis[2]; + values[2] = -quat_6_axis[3]; + values[3] = -quat_6_axis[0]; + } + //This sensor does not report an estimated heading accuracy + values[4] = 0; + if (hal_out.quat_status & INV_QUAT_3AXIS) + { + status = hal_out.quat_status & INV_NEW_DATA? 1 : 0; + } + else { + status = hal_out.accel_status & INV_NEW_DATA? 1 : 0; + } + MPL_LOGV("values:%f %f %f %f %f -%d -%lld", values[0], values[1], + values[2], values[3], values[4], status, timestamp1); + return inv_get_6_axis_gyro_accel_timestamp(hal_out.rotation_vector_6_axis_sample_rate_us, timestamp); +} + +/** +* This corresponds to Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR. +* Similar to SENSOR_TYPE_ROTATION_VECTOR, but using a magnetometer +* instead of using a gyroscope. +* Fourth element = estimated_accuracy in radians (heading confidence). +* @param[out] values Length 4. +* @param[out] accuracy is not defined. +* @param[out] timestamp in (ns) for Android. +* @return Returns 1 if the data was updated, 0 otherwise. +*/ +int inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long compass[3]; + float quat_geomagnetic[4]; + int status; + inv_time_t timestamp1; + inv_get_compass_set(compass, accuracy, ×tamp1); + inv_get_geomagnetic_quaternion_float(quat_geomagnetic, ×tamp1); + + if (quat_geomagnetic[0] >= .0) { + values[0] = quat_geomagnetic[1]; + values[1] = quat_geomagnetic[2]; + values[2] = quat_geomagnetic[3]; + values[3] = quat_geomagnetic[0]; + } else { + values[0] = -quat_geomagnetic[1]; + values[1] = -quat_geomagnetic[2]; + values[2] = -quat_geomagnetic[3]; + values[3] = -quat_geomagnetic[0]; + } + values[4] = inv_get_accel_compass_confidence_interval(); + status = hal_out.accel_status & INV_NEW_DATA? 1 : 0; + MPL_LOGV("values:%f %f %f %f %f -%d", values[0], values[1], + values[2], values[3], values[4], status); + + return inv_get_6_axis_compass_accel_timestamp(hal_out.geomagnetic_rotation_vector_sample_rate_us, timestamp); +} + +/** Compass data (uT) in body frame. +* @param[out] values Compass data in (uT), length 3. May be calibrated by having +* biases removed and sensitivity adjusted +* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate +* @param[out] timestamp Timestamp. In (ns) for Android. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + int status; + int i; + /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. + * So this is: 1 / 2^16*/ +//#define COMPASS_CONVERSION 1.52587890625e-005f + + *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]; + if (hal_out.compass_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + +/** Compass raw data (uT) in body frame. +* @param[out] values Compass data in (uT), length 3. May be calibrated by having +* biases removed and sensitivity adjusted +* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate +* @param[out] timestamp Timestamp. In (ns) for Android. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_magnetic_field_raw(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long mag[3]; + int status; + int i; + + inv_get_compass_set_raw(mag, accuracy, timestamp); + + /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. + * So this is: 1 / 2^16*/ +#define COMPASS_CONVERSION 1.52587890625e-005f + + for (i = 0; i < 3; i++) { + values[i] = (float)mag[i] * COMPASS_CONVERSION; + } + if (hal_out.compass_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} +static void inv_get_rotation_geomagnetic(float r[3][3]) +{ + long rot[9], quat_geo[4]; + float conv = 1.f / (1L<<30); + inv_time_t timestamp; + inv_get_geomagnetic_quaternion(quat_geo, ×tamp); + inv_quaternion_to_rotation(quat_geo, 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_geomagnetic(float *g) +{ + float rad2deg = (float)(180.0 / M_PI); + float R[3][3]; + inv_get_rotation_geomagnetic(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; +} + +static void inv_get_rotation_6_axis(float r[3][3]) +{ + long rot[9], quat_6_axis[4]; + float conv = 1.f / (1L<<30); + inv_time_t timestamp; + + inv_get_6axis_quaternion(quat_6_axis, ×tamp); + inv_quaternion_to_rotation(quat_6_axis, 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_6_axis(float *g) +{ + float rad2deg = (float)(180.0 / M_PI); + float R[3][3]; + + inv_get_rotation_6_axis(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; +} + +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> +* - values[0]: Azimuth, angle between the magnetic north direction +* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> +* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values +* when the z-axis moves toward the y-axis.<br> +* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive +* values when the x-axis moves toward the z-axis.<br> +* +* @note This definition is different from yaw, pitch and roll used in aviation +* where the X axis is along the long side of the plane (tail to nose). +* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() +* in conjunction with remapCoordinateSystem() and getOrientation() to compute +* these values instead. +* Important note: For historical reasons the roll angle is positive in the +* clockwise direction (mathematically speaking, it should be positive in +* the counter-clockwise direction). +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + *accuracy = (int8_t) hal_out.accuracy_quat; + google_orientation(values); + + return inv_get_9_axis_timestamp(hal_out.orientation_sample_rate_us, timestamp); +} + +int inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long accel[3]; + inv_time_t timestamp1; + inv_get_accel_set(accel, accuracy, ×tamp1); + + google_orientation_6_axis(values); + + return inv_get_6_axis_gyro_accel_timestamp(hal_out.orientation_6_axis_sample_rate_us, timestamp); +} + +int inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long compass[3]; + inv_time_t timestamp1; + inv_get_compass_set(compass, accuracy, ×tamp1); + + google_orientation_geomagnetic(values); + + return inv_get_6_axis_compass_accel_timestamp(hal_out.orientation_geomagnetic_sample_rate_us, timestamp); +} + +/** 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 +* 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; + (void) sensor_cal; + + inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, + &hal_out.nav_timestamp); + hal_out.gyro_status = sensor_cal->gyro.status; + hal_out.accel_status = sensor_cal->accel.status; + hal_out.compass_status = sensor_cal->compass.status; + hal_out.quat_status = sensor_cal->quat.status; +#if MPL_LOG_9AXIS_DEBUG + MPL_LOGV("hal_out:g=%d", hal_out.gyro_status); +#endif + // Find the highest sample rate and tie generating 9-axis to that one. + if (sensor_cal->gyro.status & INV_SENSOR_ON) { + sr = sensor_cal->gyro.sample_rate_ms; + use_sensor = 0; + } + if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { + sr = sensor_cal->accel.sample_rate_ms; + use_sensor = 1; + } + if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { + sr = sensor_cal->compass.sample_rate_ms; + use_sensor = 2; + } + if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { + sr = sensor_cal->quat.sample_rate_ms; + use_sensor = 3; + } + + // If the timestamp did not change, remove the new data flag + if (sensor_cal->gyro.timestamp_prev == sensor_cal->gyro.timestamp) { + hal_out.gyro_status &= ~INV_NEW_DATA; + } + if (sensor_cal->accel.timestamp_prev == sensor_cal->accel.timestamp) { + hal_out.accel_status &= ~INV_NEW_DATA; + } + if (sensor_cal->compass.timestamp_prev == sensor_cal->compass.timestamp) { + hal_out.compass_status &= ~INV_NEW_DATA; + } + if (sensor_cal->quat.timestamp_prev == sensor_cal->quat.timestamp) { + hal_out.quat_status &= ~INV_NEW_DATA; + } + + // 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; + } + } +#if MPL_LOG_9AXIS_DEBUG + MPL_LOGI("use_sensor=%d", use_sensor); +#endif + switch (use_sensor) { + case 0: + hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; + 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; + 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; + 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 + break; + } +#if MPL_LOG_9AXIS_DEBUG + MPL_LOGI("nav ts: %lld", hal_out.nav_timestamp); +#endif + /* 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; + +#ifndef CALIB_COMPASS_NOISE_REDUCTION + for (i = 0; i < 3; i++) { + hal_out.compass_float[i] = (float)compass[i] * COMPASS_CONVERSION; + } +#else + 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; + } + } +#endif // CALIB_COMPASS_NOISE_REDUCTION + return INV_SUCCESS; +} + +/** Turns off generation of HAL outputs. +* @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_stop_hal_outputs(void) +{ + inv_error_t result; + result = inv_unregister_data_cb(inv_generate_hal_outputs); + return result; +} + +/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() +* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_start_hal_outputs(void) +{ + inv_error_t result; + result = + inv_register_data_cb(inv_generate_hal_outputs, + INV_PRIORITY_HAL_OUTPUTS, + INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW | INV_QUAT_NEW | INV_PRESSURE_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 +* is typically not needed to be called by outside callers. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +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; +} + +/** Turns on creation and storage of HAL type results. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_enable_hal_outputs(void) +{ + inv_error_t result; + + // don't need to check the result for inv_init_hal_outputs + // since it's always INV_SUCCESS + inv_init_hal_outputs(); + + result = inv_register_mpl_start_notification(inv_start_hal_outputs); + return result; +} + +/** Turns off creation and storage of HAL type results. +*/ +inv_error_t inv_disable_hal_outputs(void) +{ + inv_error_t result; + + 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/6515/libsensors_iio/software/core/mllite/hal_outputs.h b/6515/libsensors_iio/software/core/mllite/hal_outputs.h new file mode 100644 index 0000000..797269c --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/hal_outputs.h @@ -0,0 +1,66 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_HAL_OUTPUTS_H__ +#define INV_HAL_OUTPUTS_H__ + +#ifdef __cplusplus +extern "C" { +#endif + + int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, + 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_magnetic_field_raw(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp); + + int inv_get_sensor_type_linear_acceleration(float *values, + int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, + inv_time_t * timestamp); + + int inv_get_sensor_type_orientation_6_axis(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_orientation_geomagnetic(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_rotation_vector_6_axis(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_geomagnetic_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp); + + inv_error_t inv_enable_hal_outputs(void); + inv_error_t inv_disable_hal_outputs(void); + inv_error_t inv_init_hal_outputs(void); + inv_error_t inv_start_hal_outputs(void); + inv_error_t inv_stop_hal_outputs(void); + + // Set data rates for virtual sensors + void inv_set_linear_acceleration_sample_rate(long sample_rate_us); + void inv_set_orientation_sample_rate(long sample_rate_us); + void inv_set_rotation_vector_sample_rate(long sample_rate_us); + void inv_set_gravity_sample_rate(long sample_rate_us); + void inv_set_orientation_6_axis_sample_rate(long sample_rate_us); + void inv_set_orientation_geomagnetic_sample_rate(long sample_rate_us); + void inv_set_rotation_vector_6_axis_sample_rate(long sample_rate_us); + void inv_set_geomagnetic_rotation_vector_sample_rate(long sample_rate_us); + +#ifdef __cplusplus +} +#endif + +#endif // INV_HAL_OUTPUTS_H__ diff --git a/6515/libsensors_iio/software/core/mllite/invensense.h b/6515/libsensors_iio/software/core/mllite/invensense.h new file mode 100644 index 0000000..fb8e12b --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/invensense.h @@ -0,0 +1,28 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/** + * Main header file for Invensense's basic library. + */ + +#include "data_builder.h" +#include "hal_outputs.h" +#include "message_layer.h" +#include "mlmath.h" +#include "ml_math_func.h" +#include "mpl.h" +#include "results_holder.h" +#include "start_manager.h" +#include "storage_manager.h" +#include "log.h" +#include "mlinclude.h" +//#include "..\HAL\include\mlos.h" diff --git a/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c new file mode 100644 index 0000000..649b917 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c @@ -0,0 +1,318 @@ +/** + * @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/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h new file mode 100644 index 0000000..45a35f9 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h @@ -0,0 +1,84 @@ +/** + * @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/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c new file mode 100644 index 0000000..14e0c9c --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -0,0 +1,271 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +/** + * @defgroup ML_LOAD_DMP + * + * @{ + * @file ml_load_dmp.c + * @brief functions for writing dmp firmware. + */ +#include <stdio.h> + +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-loaddmp" + +#include "ml_load_dmp.h" +#include "log.h" +#include "mlos.h" + +#define LOADDMP_LOG MPL_LOGI +#define LOADDMP_LOG MPL_LOGI + +#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) +#define DMP_CODE_SIZE 2463 + +static const unsigned char dmpMemory[DMP_CODE_SIZE] = { + /* bank # 0 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x7d, 0x00, 0x01, + 0x00, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x00, 0x83, 0x12, 0x6f, 0xff, 0xdf, 0x3b, 0x64, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x07, + 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x07, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 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, + /* bank # 1 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 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, 0x02, 0x58, 0x05, 0xdc, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc8, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0xff, + 0x08, 0x00, 0x02, 0x00, 0x04, 0x00, 0x01, 0x00, 0x80, 0x00, 0x10, 0x00, 0x20, 0x00, 0x40, 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, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 2 */ + 0x00, 0x28, 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, + 0x0b, 0xeb, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x04, 0x06, 0x31, 0x0a, 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, 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, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x05, + 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, + 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, 0x10, 0x00, 0x00, 0x00, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 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, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + /* bank # 4 */ + 0xd8, 0xb0, 0xb5, 0xb9, 0xf3, 0xa6, 0xf8, 0xf9, 0xd1, 0xd9, 0x81, 0x96, 0xf8, 0xf7, 0x3e, 0xd8, + 0xf3, 0xb1, 0x86, 0x96, 0xa3, 0x31, 0xd1, 0xda, 0xf1, 0xff, 0xd8, 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, 0xf1, 0xa9, + 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x8a, 0xa8, 0x96, 0x36, + 0x56, 0x76, 0xd8, 0xf1, 0xb0, 0x89, 0xb9, 0xa3, 0xc3, 0xc5, 0xc7, 0xb1, 0x81, 0xb4, 0x97, 0xa3, + 0x11, 0xb5, 0x93, 0xa1, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf1, 0x93, 0x81, 0xa3, 0x2d, + 0x55, 0x3d, 0xf2, 0xa6, 0xfa, 0xf9, 0xd1, 0xd9, 0xf8, 0xf1, 0x83, 0xa2, 0xc3, 0xc5, 0xc7, 0xd8, + 0xf3, 0xa2, 0xde, 0xf1, 0x82, 0x93, 0xa3, 0x2d, 0x55, 0x7d, 0x83, 0x95, 0xf5, 0xa3, 0x30, 0xd9, + 0xf3, 0xa2, 0xf8, 0xd8, 0xf5, 0xa3, 0x50, 0xd9, 0xf3, 0xa2, 0xf8, 0xd8, 0xf5, 0xa3, 0x70, 0xd9, + 0xf3, 0xa2, 0xf8, 0xd8, 0xf2, 0xb9, 0xa2, 0xf8, 0xf9, 0xd1, 0xd9, 0xa6, 0xde, 0xdf, 0xf4, 0x1c, + 0xd8, 0xf2, 0xa6, 0xf8, 0xf9, 0xd1, 0xd9, 0xf4, 0x14, 0xd8, 0xf2, 0xf9, 0xd1, 0xd9, 0xf8, 0xf4, + 0x1e, 0xd8, 0xf2, 0xf9, 0xd1, 0xd9, 0xf8, 0xf8, 0xf4, 0x14, 0xd8, 0xf4, 0x10, 0xd8, 0xf3, 0xa2, + 0xf8, 0xf9, 0xd1, 0xda, 0xf2, 0xa6, 0xf8, 0xf1, 0xa5, 0xde, 0xd8, 0xf4, 0xa3, 0x14, 0x14, 0xd8, + 0xf1, 0xa5, 0xf8, 0xa3, 0x85, 0x95, 0x09, 0xd9, 0xf1, 0xa5, 0xde, 0xf2, 0xa6, 0xf8, 0xdf, 0xd8, + 0xf4, 0xa3, 0x09, 0xd8, 0xf3, 0xa2, 0xf8, 0xf9, 0xd1, 0xf4, 0xd9, 0x08, 0x17, 0xda, 0x42, 0xf2, + /* bank # 5 */ + 0xa2, 0xde, 0xf4, 0x0b, 0xd8, 0xf1, 0xa5, 0xf8, 0xa3, 0x85, 0x95, 0x19, 0xda, 0xf4, 0x05, 0xd8, + 0xf2, 0xa6, 0xde, 0xdf, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d, 0xb2, + 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xb8, 0xa8, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xaa, + 0x88, 0x2c, 0x54, 0x7c, 0xd8, 0xf1, 0xb8, 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, + 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, + 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, + 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, + 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, + 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, + 0x29, 0x51, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, 0x58, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, + 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, 0x49, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, + 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, + 0x46, 0x66, 0xf0, 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, + 0x48, 0x31, 0xa9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, + 0x48, 0x60, 0x8c, 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, + 0x6e, 0x76, 0x7e, 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xf1, 0xb3, 0x8b, 0xb4, 0x97, 0xbb, + /* bank # 6 */ + 0xab, 0xf8, 0xf9, 0xb9, 0xa3, 0xda, 0xc3, 0xc5, 0xc7, 0xd9, 0x2d, 0x4d, 0x6d, 0xd8, 0xb0, 0x8c, + 0xb8, 0xac, 0xf8, 0xf9, 0xa8, 0xda, 0xc3, 0xc5, 0xc7, 0xd9, 0x2d, 0x4d, 0x6d, 0xd8, 0xf3, 0xb9, + 0xac, 0xde, 0xd8, 0xb1, 0xb6, 0xb9, 0xf1, 0xa8, 0xf8, 0xf3, 0xb9, 0xaa, 0xfa, 0xf9, 0xd1, 0xda, + 0xf2, 0x8a, 0xca, 0xf4, 0x0e, 0xf3, 0xb1, 0x87, 0xdd, 0xc2, 0xc2, 0xf1, 0x8c, 0xc4, 0xdc, 0xf3, + 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 0xb6, 0xba, 0xad, 0xfa, 0x8d, 0x9d, 0xab, 0x39, 0xd9, 0xad, + 0xdf, 0xf4, 0x13, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x08, 0x08, 0xf1, 0xb1, 0x83, 0xc2, 0xc4, + 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xad, 0xf8, 0x8d, 0x9d, 0xab, + 0x11, 0xd9, 0xad, 0xde, 0xf4, 0x13, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x28, 0x28, 0xf1, 0xb0, + 0x88, 0xc2, 0xc4, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xad, 0xfa, + 0x8d, 0x9d, 0xab, 0x39, 0xd9, 0xad, 0xdf, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 0xb5, 0x9a, 0x08, + 0xf2, 0xf2, 0x88, 0xc2, 0xc4, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, 0xb6, 0xba, + 0xae, 0xf8, 0x8e, 0x9e, 0xab, 0x11, 0xd9, 0xae, 0xde, 0xf4, 0x12, 0xdd, 0xf3, 0xb1, 0x87, 0xb5, + 0x9a, 0x68, 0xf2, 0xb0, 0x80, 0xc0, 0xc8, 0xc2, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb2, + 0xb6, 0xba, 0xae, 0xfa, 0x8e, 0x9e, 0xab, 0x39, 0xd9, 0xae, 0xdf, 0xf4, 0x12, 0xdd, 0xf2, 0xb1, + 0x87, 0xb5, 0x9a, 0x68, 0xf2, 0xb0, 0x80, 0xc4, 0xcc, 0xc6, 0xdc, 0xf3, 0xb9, 0xac, 0xf8, 0xd8, + 0xf3, 0xb2, 0xb6, 0xba, 0xae, 0xf8, 0x8e, 0x9e, 0xab, 0x11, 0xd9, 0xae, 0xde, 0xf4, 0x12, 0xdd, + /* bank # 7 */ + 0xf3, 0xb1, 0x87, 0xb5, 0x9a, 0x48, 0xf2, 0xb0, 0x81, 0xc0, 0xc8, 0xc2, 0xdc, 0xf3, 0xb9, 0xac, + 0xf8, 0xd8, 0xf3, 0xb2, 0xb6, 0xba, 0xae, 0xfa, 0x8e, 0x9e, 0xab, 0x39, 0xd9, 0xae, 0xdf, 0xf4, + 0x12, 0xdd, 0xf2, 0xb1, 0x87, 0xb5, 0x9a, 0x48, 0xf2, 0xb0, 0x81, 0xc4, 0xcc, 0xc6, 0xdc, 0xf3, + 0xb9, 0xac, 0xf8, 0xd8, 0xf2, 0xb0, 0x88, 0xb9, 0xa8, 0xc3, 0xc5, 0xc7, 0xb1, 0xb5, 0xb9, 0xf3, + 0xac, 0xfa, 0xf9, 0xd1, 0xda, 0xf4, 0x10, 0xd8, 0xf3, 0xac, 0xf8, 0xf9, 0xd1, 0xda, 0xf1, 0xb9, + 0xaa, 0xdf, 0xf4, 0x1c, 0xd9, 0xf4, 0x1a, 0xd8, 0xf3, 0xac, 0xf8, 0xf9, 0xd1, 0xd9, 0xf4, 0x1d, + 0xd8, 0xf2, 0x8c, 0xac, 0xf8, 0xf9, 0xd1, 0xd9, 0xc2, 0xd8, 0xf2, 0xf9, 0xd9, 0xde, 0xf4, 0x09, + 0x1d, 0x16, 0xda, 0xf2, 0xdd, 0xc6, 0xdc, 0xf4, 0x30, 0xf1, 0xb9, 0xaa, 0xdf, 0xd8, 0xf1, 0xab, + 0xfa, 0x8b, 0x9b, 0xa3, 0x69, 0xd9, 0xf4, 0x07, 0x06, 0xda, 0xf1, 0xb9, 0xab, 0xdf, 0xfe, 0xd8, + 0xf1, 0xbb, 0xb3, 0xb7, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, + 0xa7, 0x20, 0xd8, 0xf1, 0xb3, 0xb7, 0xbb, 0x87, 0xaa, 0xd0, 0xc1, 0xa7, 0x81, 0x97, 0x62, 0x93, + 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, + 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x8a, 0x6c, 0xf2, 0xb0, + 0xb4, 0xb8, 0x83, 0x93, 0xa5, 0x71, 0xdb, 0xf1, 0x8c, 0x01, 0xa3, 0xc0, 0xd8, 0xf2, 0x83, 0xa5, + 0x71, 0xd9, 0xa3, 0xd0, 0xf8, 0xf1, 0x9c, 0xa5, 0xdb, 0x21, 0x8c, 0xa3, 0xc1, 0xd8, 0xf2, 0x83, + 0xa5, 0xc7, 0xf9, 0xf1, 0x93, 0x25, 0xf2, 0xa5, 0x95, 0x49, 0xd9, 0xf3, 0xa3, 0xd0, 0xde, 0xf8, + /* bank # 8 */ + 0xf1, 0x84, 0xa5, 0xd0, 0xdb, 0x55, 0xf3, 0xa3, 0xd0, 0xde, 0xd8, 0xf2, 0xa5, 0x83, 0x95, 0x49, + 0xdb, 0xf3, 0x8e, 0x93, 0x11, 0xf1, 0x85, 0x94, 0xa5, 0x79, 0x9e, 0x83, 0xf3, 0x69, 0xa3, 0xd0, + 0xf8, 0xd8, 0xf2, 0xa5, 0x83, 0x95, 0x49, 0xd9, 0xf3, 0xa3, 0xd0, 0xc5, 0xf2, 0xa3, 0xd0, 0xde, + 0xf1, 0xa3, 0xde, 0x87, 0xc5, 0xd8, 0xf3, 0x8e, 0x93, 0xa5, 0x11, 0xdb, 0xf2, 0x8d, 0xb7, 0x92, + 0x31, 0xbb, 0xa2, 0xd0, 0xc2, 0xa8, 0xb3, 0x82, 0xb4, 0x9d, 0x21, 0xb0, 0x8d, 0xb7, 0x92, 0x49, + 0xa2, 0xdf, 0xd8, 0xf3, 0xb4, 0x9e, 0xb0, 0x83, 0xb8, 0xa5, 0x49, 0xdb, 0xf2, 0xb3, 0x82, 0x9d, + 0x41, 0x21, 0xb0, 0x84, 0xb7, 0x92, 0x09, 0xd9, 0xb3, 0x82, 0xbb, 0xa8, 0xf1, 0xca, 0x96, 0x88, + 0xa6, 0x00, 0xd8, 0xf3, 0xb7, 0x90, 0xb0, 0x83, 0xb8, 0xa5, 0x49, 0xdb, 0xf2, 0xb3, 0x82, 0xb4, + 0x9d, 0x41, 0xb0, 0x8d, 0xbb, 0xa2, 0xd0, 0xc0, 0xb3, 0x82, 0xb4, 0x9d, 0xa8, 0x21, 0xa2, 0xdf, + 0xd8, 0xf3, 0xbb, 0xb3, 0xb7, 0x90, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2, + 0xf2, 0xd9, 0x26, 0xf3, 0xa7, 0xd0, 0xdf, 0xd8, 0xf1, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, + 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0x85, 0xf5, 0x75, 0xda, + 0xff, 0xd8, 0x71, 0x80, 0xa9, 0xda, 0xf1, 0xff, 0xd8, 0xd8, 0xf1, 0xb2, 0xb7, 0xba, 0x83, 0x99, + 0xa4, 0x09, 0xda, 0xa3, 0xdf, 0xf9, 0xc7, 0xf4, 0x0b, 0xd8, 0xf1, 0xa3, 0xfb, 0xf8, 0xdb, 0xfb, + 0xd9, 0xde, 0xa3, 0xdf, 0xd8, 0xf1, 0xa3, 0xfa, 0xf9, 0xbb, 0xa9, 0xd0, 0x84, 0xda, 0xc2, 0xd9, + 0xc4, 0xd8, 0xb3, 0x82, 0xa7, 0xf3, 0xc1, 0xf2, 0x80, 0xc2, 0xf1, 0x97, 0x86, 0x49, 0x2e, 0xa6, + /* bank # 9 */ + 0xd0, 0x50, 0x96, 0x86, 0xa8, 0x75, 0xd9, 0x88, 0xa2, 0xd0, 0xf3, 0xc4, 0xc7, 0xf1, 0xda, 0x88, + 0x96, 0xa2, 0xd0, 0xf3, 0xc2, 0xc3, 0x82, 0xb4, 0x94, 0x68, 0x68, 0xf1, 0xd8, 0x80, 0xb7, 0x90, + 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8, 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, 0xa9, + 0x22, 0x26, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0xde, 0xf1, 0xa9, 0xdf, + 0x97, 0x8c, 0xa8, 0xf3, 0x79, 0xda, 0xf1, 0xf1, 0xf1, 0xf1, 0xb1, 0x88, 0xb9, 0xac, 0xd0, 0xc0, + 0xb3, 0xf3, 0xf3, 0xb9, 0xaa, 0xfa, 0xf1, 0xbb, 0xaa, 0xd0, 0xf8, 0xf4, 0x10, 0xd8, 0xf3, 0xa7, + 0xd0, 0xfa, 0x97, 0x8c, 0xa8, 0x79, 0xda, 0xf1, 0x87, 0x9a, 0xaa, 0xd0, 0x70, 0xd8, 0xf2, 0x82, + 0x92, 0xa8, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, + 0xd0, 0xde, 0xf1, 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, 0xff +}; + +#define DMP_VERSION (dmpMemory) + +inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) +{ + inv_error_t result = INV_SUCCESS; + int bytesWritten = 0; + + if (len <= 0) { + MPL_LOGE("Nothing to write"); + return INV_ERROR_FILE_WRITE; + } + else { + MPL_LOGI("dmp firmware size to write = %d", len); + } + if ( fd == NULL ) { + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(dmp, 1, len, fd); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } + else { + MPL_LOGI("Bytes written = %d", bytesWritten); + } + return result; +} + +inv_error_t inv_load_dmp(FILE *fd) +{ + inv_error_t result = INV_SUCCESS; + result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE); + return result; +} + +void read_dmp_img(char *dmp_path, char* out_file) +{ + MPL_LOGI("read_dmp_img"); + FILE *fp; + int i; + int dmpSize = DMP_CODE_SIZE; + char dmp_img[dmpSize]; + + if ((fp = fopen(dmp_path, "rb")) < 0) { + perror("dmp fail"); + } + i = fread(dmp_img, 1, dmpSize, fp); + MPL_LOGI("Result=%d", i); + fclose(fp); + fp = fopen(out_file, "wt"); + if(fp == NULL) { + MPL_LOGE("error open out file:%s", out_file); + return; + } + fprintf(fp, "char rec[]={\n"); + for(i = 0; i < dmpSize; i++) { + fprintf(fp, "0x%02x, ", dmp_img[i]); + if(((i + 1) % 16) == 0) { + fprintf(fp, "\n"); + } + } + fprintf(fp, "};\n "); + fclose(fp); +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h new file mode 100644 index 0000000..3369f37 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h @@ -0,0 +1,34 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef INV_LOAD_DMP_H +#define INV_LOAD_DMP_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Includes. +*/ +#include "mltypes.h" + +/* + APIs +*/ +inv_error_t inv_load_dmp(FILE *fd); +void read_dmp_img(char *dmp_path, char *out_file); + +#ifdef __cplusplus +} +#endif +#endif /* INV_LOAD_DMP_H */ diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.c new file mode 100644 index 0000000..bf4520d --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.c @@ -0,0 +1,351 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML_STORED_DATA + * + * @{ + * @file ml_stored_data.c + * @brief functions for reading and writing stored data sets. + * Typically, these functions process stored calibration data. + */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ +#undef MPL_LOG_TAG + +#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 "mlos.h" + +#define LOADCAL_DEBUG 0 +#define STORECAL_DEBUG 0 + +#define DEFAULT_KEY 29681 + +#define STORECAL_LOG MPL_LOGI +#define LOADCAL_LOG MPL_LOGI + +inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) +{ + FILE *fp; + 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); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } else { + MPL_LOGV("Bytes read = %d", *bytesRead); + } + +read_cal_end: + fclose(fp); + return result; +} + +inv_error_t inv_write_cal(unsigned char *cal, size_t len) +{ + FILE *fp; + int bytesWritten; + inv_error_t result = INV_SUCCESS; + + if (len <= 0) { + MPL_LOGE("Nothing to write"); + return INV_ERROR_FILE_WRITE; + } else { + MPL_LOGV("cal data size to write = %d", len); + } + fp = fopen(MLCAL_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(cal, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } else { + MPL_LOGV("Bytes written = %d", bytesWritten); + } + fclose(fp); + return result; +} + +/** + * @brief Loads a type 0 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * This calibration data is produced internally by the MPL and its + * size is 2777 bytes (header and checksum included). + * Calibration format type 1 is currently used for ITG3500 + * + * @pre inv_init_storage_manager() + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) +{ + inv_error_t result; + + LOADCAL_LOG("Entering inv_load_cal_V0\n"); + + /*if (len != expLen) { + MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + }*/ + + result = inv_load_mpl_states(calData, len); + return result; +} + +/** + * @brief Loads a type 1 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 36 bytes (header and checksum included). + * Calibration format type 1 is produced by the MPU Self Test and + * substitutes the type 0 : inv_load_cal_V0(). + * + * @pre + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) +{ + return INV_SUCCESS; +} + +/** + * @brief Loads a set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * + * @pre + * + * + * @param calData + * A pointer to an array of bytes to be parsed. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal(unsigned char *calData) +{ + int calType = 0; + int len = 0; + //int ptr; + //uint32_t chk = 0; + //uint32_t cmp_chk = 0; + + /*load_func_t loaders[] = { + inv_load_cal_V0, + inv_load_cal_V1, + }; + */ + + inv_load_cal_V0(calData, len); + + /* read the header (type and len) + len is the total record length including header and checksum */ + len = 0; + len += 16777216L * ((int)calData[0]); + len += 65536L * ((int)calData[1]); + len += 256 * ((int)calData[2]); + len += (int)calData[3]; + + calType = ((int)calData[4]) * 256 + ((int)calData[5]); + if (calType > 5) { + MPL_LOGE("Unsupported calibration file format %d. " + "Valid types 0..5\n", calType); + return INV_ERROR_INVALID_PARAMETER; + } + + /* call the proper method to read in the data */ + //return loaders[calType] (calData, len); + return 0; +} + +/** + * @brief Stores a set of calibration data. + * It generates a binary data set containing calibration data. + * The binary data set is intended to be stored into a file. + * + * @pre inv_dmp_open() + * + * @param calData + * A pointer to an array of bytes to be stored. + * @param length + * The amount of bytes available in the array. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_store_cal(unsigned char *calData, size_t length) +{ + inv_error_t res = 0; + size_t size; + + STORECAL_LOG("Entering inv_store_cal\n"); + + inv_get_mpl_state_size(&size); + + MPL_LOGV("inv_get_mpl_state_size() : size=%d", size); + + /* store data */ + res = inv_save_mpl_states(calData, size); + if(res != 0) { + MPL_LOGE("inv_save_mpl_states() failed"); + } + + STORECAL_LOG("Exiting inv_store_cal\n"); + return INV_SUCCESS; +} + +/** + * @brief Load a calibration file. + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_load_calibration(void) +{ + unsigned char *calData= NULL; + inv_error_t result = 0; + size_t bytesRead = 0; + + result = inv_read_cal(&calData, &bytesRead); + 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); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not load the calibration data - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } + +free_mem_n_exit: + inv_free(calData); + return result; +} + +/** + * @brief Store runtime calibration data to a file + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_store_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + size_t length; + + result = inv_get_mpl_state_size(&length); + 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; + } else { + MPL_LOGV("inv_get_mpl state size = %d", length); + } + + result = inv_save_mpl_states(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not save mpl states - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } else { + MPL_LOGV("calData from inv_save_mpl_states, size=%d", + strlen((char *)calData)); + } + + result = inv_write_cal(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not store calibrated data on file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + +free_mem_n_exit: + inv_free(calData); + return result; +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.h new file mode 100644 index 0000000..115b34c --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_stored_data.h @@ -0,0 +1,53 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $ + * + ******************************************************************************/ + +#ifndef INV_MPL_STORED_DATA_H +#define INV_MPL_STORED_DATA_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Includes. +*/ +#include "mltypes.h" + +/* + Defines +*/ +#define MLCAL_FILE "/data/inv_cal_data.bin" + +/* + APIs +*/ +inv_error_t inv_load_calibration(void); +inv_error_t inv_store_calibration(void); + +/* + Internal APIs +*/ +inv_error_t inv_read_cal(unsigned char **, size_t *); +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); + +/* + Other prototypes +*/ +inv_error_t inv_load_cal(unsigned char *calData); +inv_error_t inv_store_cal(unsigned char *calData, size_t length); + +#ifdef __cplusplus +} +#endif +#endif /* INV_MPL_STORED_DATA_H */ diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c new file mode 100644 index 0000000..cbf69f8 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c @@ -0,0 +1,526 @@ +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MLLITE" + +#include <string.h> +#include <stdio.h> +#include "ml_sysfs_helper.h" +#include <dirent.h> +#include <ctype.h> +#include "log.h" + +#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" + +enum PROC_SYSFS_CMD { + CMD_GET_SYSFS_PATH, + CMD_GET_DMP_PATH, + CMD_GET_CHIP_NAME, + CMD_GET_SYSFS_KEY, + CMD_GET_TRIGGER_PATH, + CMD_GET_DEVICE_NODE +}; +static char sysfs_path[100]; +static char *chip_name[] = { + "ITG3500", + "MPU6050", + "MPU9150", + "MPU3050", + "MPU6500", + "MPU9250", + "MPU6XXX", + "MPU9350", + "MPU6515", +}; +static int chip_ind; +static int initialized =0; +static int status = 0; +static int iio_initialized = 0; +static int iio_dev_num = 0; + +#define IIO_MAX_NAME_LENGTH 30 + +#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/"; + +/** + * find_type_by_name() - function to match top level types by name + * @name: top level type instance name + * @type: the type of top level instance being sort + * + * Typical types this is used for are device and trigger. + **/ +int find_type_by_name(const char *name, const char *type) +{ + const struct dirent *ent; + int number, numstrlen; + + FILE *nameFile; + DIR *dp; + char thisname[IIO_MAX_NAME_LENGTH]; + char *filename; + + dp = opendir(iio_dir); + if (dp == NULL) { + MPL_LOGE("No industrialio devices available"); + return -ENODEV; + } + + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name, ".") != 0 && + strcmp(ent->d_name, "..") != 0 && + strlen(ent->d_name) > strlen(type) && + strncmp(ent->d_name, type, strlen(type)) == 0) { + numstrlen = sscanf(ent->d_name + strlen(type), + "%d", + &number); + /* verify the next character is not a colon */ + if (strncmp(ent->d_name + strlen(type) + numstrlen, + ":", + 1) != 0) { + filename = malloc(strlen(iio_dir) + + strlen(type) + + numstrlen + + 6); + if (filename == NULL) + return -ENOMEM; + sprintf(filename, "%s%s%d/name", + iio_dir, + type, + number); + nameFile = fopen(filename, "r"); + if (!nameFile) + continue; + free(filename); + fscanf(nameFile, "%s", thisname); + if (strcmp(name, thisname) == 0) + return number; + fclose(nameFile); + } + } + } + return -ENODEV; +} + +/* mode 0: search for which chip in the system and fill sysfs path + mode 1: return event number + */ +static int parsing_proc_input(int mode, char *name){ + const char input[] = "/proc/bus/input/devices"; + char line[4096], d; + char tmp[100]; + FILE *fp; + int i, j, result, find_flag; + int event_number = -1; + int input_number = -1; + + if(NULL == (fp = fopen(input, "rt")) ){ + return -1; + } + result = 1; + find_flag = 0; + while(result != 0 && find_flag < 2){ + i = 0; + d = 0; + memset(line, 0, 100); + while(d != '\n'){ + result = fread(&d, 1, 1, fp); + if(result == 0){ + line[0] = 0; + break; + } + sprintf(&line[i], "%c", d); + i ++; + } + if(line[0] == 'N'){ + i = 1; + while(line[i] != '"'){ + i++; + } + i++; + j = 0; + find_flag = 0; + if (mode == 0){ + while(j < CHIP_NUM){ + if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){ + find_flag = 1; + chip_ind = j; + } + j++; + } + } else if (mode != 0){ + if(!memcmp(&line[i], name, strlen(name))){ + find_flag = 1; + } + } + } + if(find_flag){ + if(mode == 0){ + if(line[0] == 'S'){ + memset(tmp, 0, 100); + i =1; + while(line[i] != '=') i++; + i++; + j = 0; + while(line[i] != '\n'){ + tmp[j] = line[i]; + i ++; j++; + } + sprintf(sysfs_path, "%s%s", "/sys", tmp); + find_flag++; + } + } else if(mode == 1){ + if(line[0] == 'H') { + i = 2; + while(line[i] != '=') i++; + while(line[i] != 't') i++; + i++; + event_number = 0; + while(line[i] != '\n'){ + if(line[i] >= '0' && line[i] <= '9') + event_number = event_number*10 + line[i]-0x30; + i ++; + } + find_flag ++; + } + } else if (mode == 2) { + if(line[0] == 'S'){ + memset(tmp, 0, 100); + i =1; + while(line[i] != '=') i++; + i++; + j = 0; + while(line[i] != '\n'){ + tmp[j] = line[i]; + i ++; j++; + } + input_number = 0; + if(tmp[j-2] >= '0' && tmp[j-2] <= '9') + input_number += (tmp[j-2]-0x30)*10; + if(tmp[j-1] >= '0' && tmp[j-1] <= '9') + input_number += (tmp[j-1]-0x30); + find_flag++; + } + } + } + } + fclose(fp); + if(find_flag == 0){ + return -1; + } + if(0 == mode) + status = 1; + if (mode == 1) + return event_number; + if (mode == 2) + return input_number; + return 0; + +} +static void init_iio() { + int i, j; + char iio_chip[10]; + int dev_num; + for(j=0; j< CHIP_NUM; j++) { + for (i=0; i<strlen(chip_name[j]); i++) { + iio_chip[i] = tolower(chip_name[j][i]); + } + iio_chip[strlen(chip_name[j])] = '\0'; + dev_num = find_type_by_name(iio_chip, "iio:device"); + if(dev_num >= 0) { + iio_initialized = 1; + iio_dev_num = dev_num; + chip_ind = j; + } + } +} + +static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data) +{ + char key_path[100]; + FILE *fp; + int i, result; + if(initialized == 0){ + parsing_proc_input(0, NULL); + initialized = 1; + } + if(initialized && status == 0) { + init_iio(); + if (iio_initialized == 0) + return -1; + } + + memset(key_path, 0, 100); + switch(cmd){ + case CMD_GET_SYSFS_PATH: + if (iio_initialized == 1) + sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num); + else + sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu"); + break; + case CMD_GET_DMP_PATH: + if (iio_initialized == 1) + sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num); + else + sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware"); + break; + case CMD_GET_CHIP_NAME: + sprintf(data, "%s", chip_name[chip_ind]); + break; + case CMD_GET_TRIGGER_PATH: + sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num); + break; + case CMD_GET_DEVICE_NODE: + sprintf(data, "/dev/iio:device%d", iio_dev_num); + break; + case CMD_GET_SYSFS_KEY: + memset(key_path, 0, 100); + if (iio_initialized == 1) + sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num); + else + sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key"); + + if((fp = fopen(key_path, "rt")) == NULL) + return -1; + for(i=0;i<16;i++){ + fscanf(fp, "%02x", &result); + data[i] = (char)result; + } + + fclose(fp); + break; + default: + break; + } + return 0; +} + +int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name) +{ + const struct dirent *ent; + int number, numstrlen; + + FILE *nameFile; + DIR *dp; + char *filename; + + dp = opendir(iio_dir); + if (dp == NULL) { + MPL_LOGE("No industrialio devices available"); + return -ENODEV; + } + + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name, ".") != 0 && + strcmp(ent->d_name, "..") != 0 && + strlen(ent->d_name) > strlen(type) && + strncmp(ent->d_name, type, strlen(type)) == 0) { + numstrlen = sscanf(ent->d_name + strlen(type), + "%d", + &number); + /* verify the next character is not a colon */ + if (strncmp(ent->d_name + strlen(type) + numstrlen, + ":", + 1) != 0) { + filename = malloc(strlen(iio_dir) + + strlen(type) + + numstrlen + + 6 + + strlen(sensor_type)); + if (filename == NULL) + return -ENOMEM; + sprintf(filename, "%s%s%d/%s", + iio_dir, + type, + number, + sensor_type); + nameFile = fopen(filename, "r"); + MPL_LOGI("sensor type path: %s\n", filename); + free(filename); + //fscanf(nameFile, "%s", thisname); + //if (strcmp(name, thisname) == 0) { + if(nameFile == NULL) { + MPL_LOGI("keeps searching"); + continue; + } else{ + MPL_LOGI("found directory"); + } + filename = malloc(strlen(iio_dir) + + strlen(type) + + numstrlen + + 6); + sprintf(filename, "%s%s%d/name", + iio_dir, + type, + number); + nameFile = fopen(filename, "r"); + MPL_LOGI("name path: %s\n", filename); + free(filename); + if (!nameFile) + continue; + fscanf(nameFile, "%s", sensor_name); + MPL_LOGI("name found: %s now test for mpuxxxx", sensor_name); + if( !strncmp("mpu",sensor_name, 3) ) { + char secondaryFileName[200]; + sprintf(secondaryFileName, "%s%s%d/secondary_name", + iio_dir, + type, + number); + nameFile = fopen(secondaryFileName, "r"); + MPL_LOGI("name path: %s\n", secondaryFileName); + if(!nameFile) + continue; + fscanf(nameFile, "%s", sensor_name); + MPL_LOGI("secondary name found: %s\n", sensor_name); + } + else { + fscanf(nameFile, "%s", sensor_name); + MPL_LOGI("name found: %s\n", sensor_name); + } + return 0; + //} + fclose(nameFile); + } + } + } + return -ENODEV; +} + +/** + * @brief return sysfs key. if the key is not available + * return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the key + * It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_sysfs_key(unsigned char *key) +{ + if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return the sysfs path. If the path is not + * found yet. return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the sysfs + * path. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_sysfs_path(char *name) +{ + if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +inv_error_t inv_get_sysfs_abs_path(char *name) +{ + strcpy(name, MPU_SYSFS_ABS_PATH); + return INV_SUCCESS; +} + +/** + * @brief return the dmp file path. If the path is not + * found yet. return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the dmp file + * path. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_dmpfile(char *name) +{ + if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} +/** + * @brief return the chip name. If the chip is not + * found yet. return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the chip name + * path(8 bytes). It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_chip_name(char *name) +{ + if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} +/** + * @brief return event handler number. If the handler number is not found + * return false. the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the chip name + * path(8 bytes). It should be zeroed before calling this function. + * Or it could have unpredicable result. + * @int *num: event number store + */ +inv_error_t inv_get_handler_number(const char *name, int *num) +{ + initialized = 0; + if ((*num = parsing_proc_input(1, (char *)name)) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return input number. If the handler number is not found + * return false. the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the chip name + * path(8 bytes). It should be zeroed before calling this function. + * Or it could have unpredicable result. + * @int *num: input number store + */ +inv_error_t inv_get_input_number(const char *name, int *num) +{ + initialized = 0; + if ((*num = parsing_proc_input(2, (char *)name)) < 0) + return INV_ERROR_NOT_OPENED; + else { + return INV_SUCCESS; + } +} + +/** + * @brief return iio trigger name. If iio is not initialized, return false. + * So the return must be checked to make sure the numeber is valid. + * @unsigned char *name: This should be array big enough to hold the trigger + * name. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_iio_trigger_path(const char *name) +{ + if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return iio device node. If iio is not initialized, return false. + * So the return must be checked to make sure the numeber is valid. + * @unsigned char *name: This should be array big enough to hold the device + * node. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_iio_device_node(const char *name) +{ + if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} diff --git a/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h new file mode 100644 index 0000000..184d3b2 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h @@ -0,0 +1,37 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id$ + * + ******************************************************************************/ + +#ifndef MLDMP_SYSFS_HELPER_H__ +#define MLDMP_SYSFS_HELPER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "invensense.h" + +int find_type_by_name(const char *name, const char *type); +int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name); +inv_error_t inv_get_sysfs_path(char *name); +inv_error_t inv_get_sysfs_abs_path(char *name); +inv_error_t inv_get_dmpfile(char *name); +inv_error_t inv_get_chip_name(char *name); +inv_error_t inv_get_sysfs_key(unsigned char *key); +inv_error_t inv_get_handler_number(const char *name, int *num); +inv_error_t inv_get_input_number(const char *name, int *num); +inv_error_t inv_get_iio_trigger_path(const char *name); +inv_error_t inv_get_iio_device_node(const char *name); + +#ifdef __cplusplus +} +#endif +#endif /* MLDMP_SYSFS_HELPER_H__ */ diff --git a/6515/libsensors_iio/software/core/mllite/linux/mlos.h b/6515/libsensors_iio/software/core/mllite/linux/mlos.h new file mode 100644 index 0000000..d4f8912 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/mlos.h @@ -0,0 +1,99 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include <stdio.h> +#endif +#include <pthread.h> + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(LINUX) || defined(__KERNEL__) +typedef pthread_mutex_t* 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); + + inv_error_t inv_destroy_mutex(HANDLE handle); + + void inv_sleep(int mSecs); + unsigned long 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/6515/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/6515/libsensors_iio/software/core/mllite/linux/mlos_linux.c new file mode 100644 index 0000000..5424508 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/linux/mlos_linux.c @@ -0,0 +1,190 @@ +/* + $License: + Copyright (C) 2012 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 <errno.h> + +#include "stdint_invensense.h" +#include "mlos.h" + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param num_bytes number of bytes + * @return pointer to allocated space + */ +void *inv_malloc(unsigned int num_bytes) +{ + // Allocate space. + void *alloc_ptr = malloc(num_bytes); + return alloc_ptr; +} + + +/** + * @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); + 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 m_secs) +{ + usleep(m_secs * 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); +} + +/** @} */ + diff --git a/6515/libsensors_iio/software/core/mllite/message_layer.c b/6515/libsensors_iio/software/core/mllite/message_layer.c new file mode 100644 index 0000000..d266d80 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/message_layer.c @@ -0,0 +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; +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/message_layer.h b/6515/libsensors_iio/software/core/mllite/message_layer.h new file mode 100644 index 0000000..321b5ee --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/message_layer.h @@ -0,0 +1,49 @@ +/* + $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) +/** Sensor fusion has switched from 9- to 6-axes + because of a magnetic disturbance */ +#define INV_MSG_6X_SF_EVENT (0x020) +/** Compass accuracy has dropped has dropped to 0 + because of a magnetic disturbance */ +#define INV_MSG_HEADING_NOT_ACCURATE_EVENT (0x40) +/** A setting of the factory gyro bias has occured */ +#define INV_MSG_NEW_FGB_EVENT (0x80) +/** A setting of the factory accel bias has occured */ +#define INV_MSG_NEW_FAB_EVENT (0x100) + +#ifdef WIN32 +#define INV_MSG_NEW_DMP_QUAT_WRITE_EVENT (0x200) +#endif + +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/6515/libsensors_iio/software/core/mllite/ml_math_func.c b/6515/libsensors_iio/software/core/mllite/ml_math_func.c new file mode 100644 index 0000000..88e9934 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/ml_math_func.c @@ -0,0 +1,1078 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/** + * @defgroup ML_MATH_FUNC ml_math_func + * @brief Motion Library - Math Functions + * Common math functions the Motion Library + * + * @{ + * @file ml_math_func.c + * @brief Math Functions. + */ + +#include "mlmath.h" +#include "ml_math_func.h" +#include "mlinclude.h" +#include <string.h> + +/** @internal + * Does the cross product of compass by gravity, then converts that + * to the world frame using the quaternion, then computes the angle that + * is made. + * + * @param[in] compass Compass Vector (Body Frame), length 3 + * @param[in] grav Gravity Vector (Body Frame), length 3 + * @param[in] quat Quaternion, Length 4 + * @return Angle Cross Product makes after quaternion rotation. + */ +float inv_compass_angle(const long *compass, const long *grav, const float *quat) +{ + float cgcross[4], q1[4], q2[4], qi[4]; + float angW; + + // Compass cross Gravity + cgcross[0] = 0.f; + cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; + cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; + cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; + + // Now convert cross product into world frame + inv_q_multf(quat, cgcross, q1); + inv_q_invertf(quat, qi); + inv_q_multf(q1, qi, q2); + + // Protect against atan2 of 0,0 + if ((q2[2] == 0.f) && (q2[1] == 0.f)) + return 0.f; + + // This is the unfiltered heading correction + angW = -atan2f(q2[2], q2[1]); + return angW; +} + +/** + * @brief The gyro data magnitude squared : + * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. + * @param[in] gyro Gyro data scaled with 1 dps = 2^16 + * @return the computed magnitude squared output of the gyroscope. + */ +unsigned long inv_get_gyro_sum_of_sqr(const long *gyro) +{ + unsigned long gmag = 0; + long temp; + int kk; + + for (kk = 0; kk < 3; ++kk) { + temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); + gmag += temp * temp; + } + + return gmag; +} + +/** Performs a multiply and shift by 29. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>29 +*/ +long inv_q29_mult(long a, long b) +{ +#ifdef UMPL_ELIMINATE_64BIT + long result; + result = (long)((float)a * b / (1L << 29)); + return result; +#else + long long temp; + long result; + temp = (long long)a * b; + result = (long)(temp >> 29); + return result; +#endif +} + +/** Performs a multiply and shift by 30. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>30 +*/ +long inv_q30_mult(long a, long b) +{ +#ifdef UMPL_ELIMINATE_64BIT + long result; + result = (long)((float)a * b / (1L << 30)); + return result; +#else + long long temp; + long result; + temp = (long long)a * b; + result = (long)(temp >> 30); + return result; +#endif +} + +#ifndef UMPL_ELIMINATE_64BIT +long inv_q30_div(long a, long b) +{ + long long temp; + long result; + temp = (((long long)a) << 30) / b; + result = (long)temp; + return result; +} +#endif + +/** Performs a multiply and shift by shift. These are good functions to write + * in assembly on with devices with small memory where you want to get rid of + * the long long which some assemblers don't handle well + * @param[in] a First multicand + * @param[in] b Second multicand + * @param[in] shift Shift amount after multiplying + * @return ((long long)a*b)<<shift +*/ +#ifndef UMPL_ELIMINATE_64BIT +long inv_q_shift_mult(long a, long b, int shift) +{ + long result; + result = (long)(((long long)a * b) >> shift); + return result; +} +#endif + +/** Performs a fixed point quaternion multiply. +* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled +* to 2^30 +* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled +* to 2^30 +* @param[out] qProd Product after quaternion multiply. Length 4. +* 1.0 scaled to 2^30. +*/ +void inv_q_mult(const long *q1, const long *q2, long *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) - + inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]); + + qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) + + inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]); + + qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) + + inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]); + + qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) - + inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]); +} + +/** Performs a fixed point quaternion addition. +* @param[in] q1 First Quaternion term, length 4. 1.0 scaled +* to 2^30 +* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled +* to 2^30 +* @param[out] qSum Sum after quaternion summation. Length 4. +* 1.0 scaled to 2^30. +*/ +void inv_q_add(long *q1, long *q2, long *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_vector_normalize(long *vec, int length) +{ + INVENSENSE_FUNC_START; + double normSF = 0; + int ii; + for (ii = 0; ii < length; ii++) { + normSF += + inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]); + } + if (normSF > 0) { + normSF = 1 / sqrt(normSF); + for (ii = 0; ii < length; ii++) { + vec[ii] = (int)((double)vec[ii] * normSF); + } + } else { + vec[0] = 1073741824L; + for (ii = 1; ii < length; ii++) { + vec[ii] = 0; + } + } +} + +void inv_q_normalize(long *q) +{ + INVENSENSE_FUNC_START; + inv_vector_normalize(q, 4); +} + +void inv_q_invert(const long *q, long *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +double quaternion_to_rotation_angle(const long *quat) { + double quat0 = (double )quat[0] / 1073741824; + if (quat0 > 1.0f) { + quat0 = 1.0; + } else if (quat0 < -1.0f) { + quat0 = -1.0; + } + + return acos(quat0)*2*180/M_PI; +} + +/** Rotates a 3-element vector by Rotation defined by Q +*/ +void inv_q_rotate(const long *q, const long *in, long *out) +{ + long q_temp1[4], q_temp2[4]; + long in4[4], out4[4]; + + // Fixme optimize + in4[0] = 0; + memcpy(&in4[1], in, 3 * sizeof(long)); + inv_q_mult(q, in4, q_temp1); + inv_q_invert(q, q_temp2); + inv_q_mult(q_temp1, q_temp2, out4); + memcpy(out, &out4[1], 3 * sizeof(long)); +} + +void inv_q_multf(const float *q1, const float *q2, float *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = + (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); + qProd[1] = + (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); + qProd[2] = + (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); + qProd[3] = + (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); +} + +void inv_q_addf(const float *q1, const float *q2, float *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_q_normalizef(float *q) +{ + INVENSENSE_FUNC_START; + float normSF = 0; + float xHalf = 0; + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (normSF < 2) { + xHalf = 0.5f * normSF; + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + q[0] *= normSF; + q[1] *= normSF; + q[2] *= normSF; + q[3] *= normSF; + } else { + q[0] = 1.0; + q[1] = 0.0; + q[2] = 0.0; + q[3] = 0.0; + } + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); +} + +/** Performs a length 4 vector normalization with a square root. +* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero. +*/ +void inv_q_norm4(float *q) +{ + float mag; + mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (mag) { + q[0] /= mag; + q[1] /= mag; + q[2] /= mag; + q[3] /= mag; + } else { + q[0] = 1.f; + q[1] = 0.f; + q[2] = 0.f; + q[3] = 0.f; + } +} + +void inv_q_invertf(const float *q, float *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +/** + * Converts a quaternion to a rotation matrix. + * @param[in] quat 4-element quaternion in fixed point. One is 2^30. + * @param[out] rot Rotation matrix in fixed point. One is 2^30. The + * First 3 elements of the rotation matrix, represent + * the first row of the matrix. Rotation matrix multiplied + * by a 3 element column vector transform a vector from Body + * to World. + */ +void inv_quaternion_to_rotation(const long *quat, long *rot) +{ + rot[0] = + inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], + quat[0]) - + 1073741824L; + rot[1] = + inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); + rot[2] = + inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); + rot[3] = + inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); + rot[4] = + inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], + quat[0]) - + 1073741824L; + rot[5] = + inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); + rot[6] = + inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); + rot[7] = + inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); + rot[8] = + inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], + quat[0]) - + 1073741824L; +} + +/** + * Converts a quaternion to a rotation vector. A rotation vector is + * a method to represent a 4-element quaternion vector in 3-elements. + * To get the quaternion from the 3-elements, The last 3-elements of + * the quaternion will be the given rotation vector. The first element + * of the quaternion will be the positive value that will be required + * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units. + * @param[in] quat 4-element quaternion in fixed point. One is 2^30. + * @param[out] rot Rotation vector in fixed point. One is 2^30. + */ +void inv_quaternion_to_rotation_vector(const long *quat, long *rot) +{ + rot[0] = quat[1]; + rot[1] = quat[2]; + rot[2] = quat[3]; + + if (quat[0] < 0.0) { + rot[0] = -rot[0]; + rot[1] = -rot[1]; + rot[2] = -rot[2]; + } +} + +/** Converts a 32-bit long to a big endian byte stream */ +unsigned char *inv_int32_to_big8(long x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 24) & 0xff); + big8[1] = (unsigned char)((x >> 16) & 0xff); + big8[2] = (unsigned char)((x >> 8) & 0xff); + big8[3] = (unsigned char)(x & 0xff); + return big8; +} + +/** Converts a big endian byte stream into a 32-bit long */ +long inv_big8_to_int32(const unsigned char *big8) +{ + long x; + x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) + | ((long)big8[3]); + return x; +} + +/** Converts a big endian byte stream into a 16-bit integer (short) */ +short inv_big8_to_int16(const unsigned char *big8) +{ + short x; + x = ((short)big8[0] << 8) | ((short)big8[1]); + return x; +} + +/** Converts a little endian byte stream into a 16-bit integer (short) */ +short inv_little8_to_int16(const unsigned char *little8) +{ + short x; + x = ((short)little8[1] << 8) | ((short)little8[0]); + return x; +} + +/** Converts a 16-bit short to a big endian byte stream */ +unsigned char *inv_int16_to_big8(short x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 8) & 0xff); + big8[1] = (unsigned char)(x & 0xff); + return big8; +} + +void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 6 * k + l) = *(a + 6 * i + j); + } + } + *n = *n - 1; +} + +void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 6 * k + l) = *(a + 6 * i + j); + } + } + *n = *n - 1; +} + +float inv_matrix_det(float *p, int *n) +{ + float d[6][6], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 7) - *(p + 1) ** (p + 6)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_inc(p, &d[0][0], n, i, j); + sum = + sum + *(p + 6 * i + j) * SIGNM(i + + j) * + inv_matrix_det(&d[0][0], n); + } + + return (sum); +} + +double inv_matrix_detd(double *p, int *n) +{ + double d[6][6], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 7) - *(p + 1) ** (p + 6)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_incd(p, &d[0][0], n, i, j); + sum = + sum + *(p + 6 * i + j) * SIGNM(i + + j) * + inv_matrix_detd(&d[0][0], n); + } + + return (sum); +} + +/** Wraps angle from (-M_PI,M_PI] + * @param[in] ang Angle in radians to wrap + * @return Wrapped angle from (-M_PI,M_PI] + */ +float inv_wrap_angle(float ang) +{ + if (ang > M_PI) + return ang - 2 * (float)M_PI; + else if (ang <= -(float)M_PI) + return ang + 2 * (float)M_PI; + else + return ang; +} + +/** Finds the minimum angle difference ang1-ang2 such that difference + * is between [-M_PI,M_PI] + * @param[in] ang1 + * @param[in] ang2 + * @return angle difference ang1-ang2 + */ +float inv_angle_diff(float ang1, float ang2) +{ + float d; + ang1 = inv_wrap_angle(ang1); + ang2 = inv_wrap_angle(ang2); + d = ang1 - ang2; + if (d > M_PI) + d -= 2 * (float)M_PI; + else if (d < -(float)M_PI) + d += 2 * (float)M_PI; + return d; +} + +/** bernstein hash, derived from public domain source */ +uint32_t inv_checksum(const unsigned char *str, int len) +{ + uint32_t hash = 5381; + int i, c; + + for (i = 0; i < len; i++) { + c = *(str + i); + hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ + } + + return hash; +} + +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; +} + + +/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. +* @param[in] mtx Orientation matrix to convert to a scalar. +* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the +* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent +* the column the one is on for the second row with bit number 5 being the sign. +* The next 2 bits (6 and 7) represent the column the one is on for the third row with +* bit number 8 being the sign. In binary the identity matrix would therefor be: +* 010_001_000 or 0x88 in hex. +*/ +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) +{ + + unsigned short scalar; + + /* + XYZ 010_001_000 Identity Matrix + XZY 001_010_000 + YXZ 010_000_001 + YZX 000_010_001 + ZXY 001_000_010 + ZYX 000_001_010 + */ + + scalar = inv_row_2_scale(mtx); + scalar |= inv_row_2_scale(mtx + 3) << 3; + scalar |= inv_row_2_scale(mtx + 6) << 6; + + return scalar; +} + +/** Uses the scalar orientation value to convert from chip frame to body frame +* @param[in] orientation A scalar that represent how to go from chip to body frame +* @param[in] input Input vector, length 3 +* @param[out] output Output vector, length 3 +*/ +void inv_convert_to_body(unsigned short orientation, const long *input, long *output) +{ + output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004); + output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020); + output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100); +} + +/** Uses the scalar orientation value to convert from body frame to chip frame +* @param[in] orientation A scalar that represent how to go from chip to body frame +* @param[in] input Input vector, length 3 +* @param[out] output Output vector, length 3 +*/ +void inv_convert_to_chip(unsigned short orientation, const long *input, long *output) +{ + output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004); + output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020); + output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100); +} + + +/** Uses the scalar orientation value to convert from chip frame to body frame and +* apply appropriate scaling. +* @param[in] orientation A scalar that represent how to go from chip to body frame +* @param[in] sensitivity Sensitivity scale +* @param[in] input Input vector, length 3 +* @param[out] output Output vector, length 3 +*/ +void inv_convert_to_body_with_scale(unsigned short orientation, + long sensitivity, + const long *input, long *output) +{ + output[0] = inv_q30_mult(input[orientation & 0x03] * + SIGNSET(orientation & 0x004), sensitivity); + output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] * + SIGNSET(orientation & 0x020), sensitivity); + output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] * + SIGNSET(orientation & 0x100), sensitivity); +} + +/** find a norm for a vector +* @param[in] a vector [3x1] +* @param[out] output the norm of the input vector +*/ +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]; +} + +void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut) { + // matrix format + // [ 0 3 6; + // 1 4 7; + // 2 5 8] + + // vector format: [0 1 2]^T; + int i, j; + long temp; + + for (i=0; i<3; i++) { + temp = 0; + for (j=0; j<3; j++) { + temp += inv_q30_mult(matrix[i+j*3], vecIn[j]); + } + vecOut[i] = temp; + } +} + +//============== 1/sqrt(x), 1/x, sqrt(x) Functions ================================ + +/** Calculates 1/square-root of a fixed-point number (30 bit mantissa, positive): Q1.30 +* Input must be a positive scaled (2^30) integer +* The number is scaled to lie between a range in which a Newton-Raphson +* iteration works best. Corresponding square root of the power of two is returned. +* Caller must scale final result by 2^rempow (while avoiding overflow). +* @param[in] x0, length 1 +* @param[out] rempow, length 1 +* @return scaledSquareRoot on success or zero. +*/ +long inv_inverse_sqrt(long x0, int*rempow) +{ + //% Inverse sqrt NR in the neighborhood of 1.3>x>=0.65 + //% x(k+1) = x(k)*(3 - x0*x(k)^2) + + //% Seed equals 1. Works best in this region. + //xx0 = int32(1*2^30); + + long oneoversqrt2, oneandhalf, x0_2; + unsigned long xx; + int pow2, sq2scale, nr_iters; + //long upscale, sqrt_upscale, upsclimit; + //long downscale, sqrt_downscale, downsclimit; + + // Precompute some constants for efficiency + //% int32(2^30*1/sqrt(2)) + oneoversqrt2=759250125L; + //% int32(1.5*2^30); + oneandhalf=1610612736L; + + //// Further scaling into optimal region saves one or more NR iterations. Maps into region (.9, 1.1) + //// int32(0.9/log(2)*2^30) + //upscale = 1394173804L; + //// int32(sqrt(0.9/log(2))*2^30) + //sqrt_upscale = 1223512453L; + // // int32(1.1*log(2)/.9*2^30) + //upsclimit = 909652478L; + //// int32(1.1/log(4)*2^30) + //downscale = 851995103L; + //// int32(sqrt(1.1/log(4))*2^30) + //sqrt_downscale = 956463682L; + // // int32(0.9*log(4)/1.1*2^30) + //downsclimit = 1217881829L; + + nr_iters = test_limits_and_scale(&x0, &pow2); + + sq2scale=pow2%2; // Find remainder. Is it even or odd? + + + // Further scaling to decrease NR iterations + // With the mapping below, 89% of calculations will require 2 NR iterations or less. + // TBD + + + x0_2 = x0 >>1; // This scaling incorporates factor of 2 in NR iteration below. + // Initial condition starts at 1: xx=(1L<<30); + // First iteration is simple: Instead of initializing xx=1, assign to result of first iteration: + // xx= (3/2-x0/2); + //% NR formula: xx=xx*(3/2-x0*xx*xx/2); = xx*(1.5 - (x0/2)*xx*xx) + // Initialize NR (first iteration). Note we are starting with xx=1, so the first iteration becomes an initialization. + xx = oneandhalf - x0_2; + if ( nr_iters>=2 ) { + // Second NR iteration + xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) ); + if ( nr_iters==3 ) { + // Third NR iteration. + xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) ); + // Fourth NR iteration: Not needed due to single precision. + } + } + if (sq2scale) { + *rempow = (pow2>>1) + 1; // Account for sqrt(2) in denominator, note we multiply if s2scale is true + return (inv_q30_mult(xx,oneoversqrt2)); + } + else { + *rempow = pow2>>1; + return xx; + } +} + + +/** Calculates square-root of a fixed-point number (30 bit mantissa, positive) +* Input must be a positive scaled (2^30) integer +* The number is scaled to lie between a range in which a Newton-Raphson +* iteration works best. +* @param[in] x0, length 1 +* @return scaledSquareRoot on success or zero. **/ +long inv_fast_sqrt(long x0) +{ + + //% Square-Root with NR in the neighborhood of 1.3>x>=0.65 (log(2) <= x <= log(4) ) + // Two-variable NR iteration: + // Initialize: a=x; c=x-1; + // 1st Newton Step: a=a-a*c/2; ( or: a = x - x*(x-1)/2 ) + // Iterate: c = c*c*(c-3)/4 + // a = a - a*c/2 --> reevaluating c at this step gives error of approximation + + //% Seed equals 1. Works best in this region. + //xx0 = int32(1*2^30); + + long sqrt2, oneoversqrt2, one_pt5; + long xx, cc; + int pow2, sq2scale, nr_iters; + + // Return if input is zero. Negative should really error out. + if (x0 <= 0L) { + return 0L; + } + + sqrt2 =1518500250L; + oneoversqrt2=759250125L; + one_pt5=1610612736L; + + nr_iters = test_limits_and_scale(&x0, &pow2); + + sq2scale = 0; + if (pow2 > 0) + sq2scale=pow2%2; // Find remainder. Is it even or odd? + pow2 = pow2-sq2scale; // Now pow2 is even. Note we are adding because result is scaled with sqrt(2) + + // Sqrt 1st NR iteration + cc = x0 - (1L<<30); + xx = x0 - (inv_q30_mult(x0, cc)>>1); + if ( nr_iters>=2 ) { + // Sqrt second NR iteration + // cc = cc*cc*(cc-3)/4; = cc*cc*(cc/2 - 3/2)/2; + // cc = ( cc*cc*((cc>>1) - onePt5) ) >> 1 + cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1; + xx = xx - (inv_q30_mult(xx, cc)>>1); + if ( nr_iters==3 ) { + // Sqrt third NR iteration + cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1; + xx = xx - (inv_q30_mult(xx, cc)>>1); + } + } + if (sq2scale) + xx = inv_q30_mult(xx,oneoversqrt2); + // Scale the number with the half of the power of 2 scaling + if (pow2>0) + xx = (xx >> (pow2>>1)); + else if (pow2 == -1) + xx = inv_q30_mult(xx,sqrt2); + return xx; +} + +/** Calculates 1/x of a fixed-point number (30 bit mantissa) +* Input must be a scaled (2^30) integer (+/-) +* The number is scaled to lie between a range in which a Newton-Raphson +* iteration works best. Corresponding multiplier power of two is returned. +* Caller must scale final result by 2^pow (while avoiding overflow). +* @param[in] x, length 1 +* @param[out] pow, length 1 +* @return scaledOneOverX on success or zero. +*/ +long inv_one_over_x(long x0, int*pow) +{ + //% NR for 1/x in the neighborhood of log(2) => x => log(4) + //% y(k+1)=y(k)*(2 \ 96 x0*y(k)) + //% with y(0) = 1 as the starting value for NR + + long two, xx; + int numberwasnegative, nr_iters, did_upscale, did_downscale; + + long upscale, downscale, upsclimit, downsclimit; + + *pow = 0; + // Return if input is zero. + if (x0 == 0L) { + return 0L; + } + + // This is really (2^31-1), i.e. 1.99999... . + // Approximation error is 1e-9. Note 2^31 will overflow to sign bit, so it can't be used here. + two = 2147483647L; + + // int32(0.92/log(2)*2^30) + upscale = 1425155444L; + // int32(1.08/upscale*2^30) + upsclimit = 873697834L; + + // int32(1.08/log(4)*2^30) + downscale = 836504283L; + // int32(0.92/downscale*2^30) + downsclimit = 1268000423L; + + // Algorithm is intended to work with positive numbers only. Change sign: + numberwasnegative = 0; + if (x0 < 0L) { + numberwasnegative = 1; + x0 = -x0; + } + + nr_iters = test_limits_and_scale(&x0, pow); + + did_upscale=0; + did_downscale=0; + // Pre-scaling to reduce NR iterations and improve accuracy: + if (x0<=upsclimit) { + x0 = inv_q30_mult(x0, upscale); + did_upscale = 1; + // The scaling ALWAYS leaves the number in the 2-NR iterations region: + nr_iters = 2; + // Is x0 in the single NR iteration region (0.994, 1.006) ? + if (x0 > 1067299373 && x0 < 1080184275) + nr_iters = 1; + } else if (x0>=downsclimit) { + x0 = inv_q30_mult(x0, downscale); + did_downscale = 1; + // The scaling ALWAYS leaves the number in the 2-NR iterations region: + nr_iters = 2; + // Is x0 in the single NR iteration region (0.994, 1.006) ? + if (x0 > 1067299373 && x0 < 1080184275) + nr_iters = 1; + } + + xx = (two - x0) + 1; // Note 2 will overflow so the computation (2-x) is done with "two" == (2^30-1) + // First NR iteration + xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 ); + if ( nr_iters>=2 ) { + // Second NR iteration + xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 ); + if ( nr_iters==3 ) { + // THird NR iteration. + xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 ); + // Fourth NR iteration: Not needed due to single precision. + } + } + + // Post-scaling + if (did_upscale) + xx = inv_q30_mult( xx, upscale); + else if (did_downscale) + xx = inv_q30_mult( xx, downscale); + + if (numberwasnegative) + xx = -xx; + return xx; +} + +/** Auxiliary function used by inv_OneOverX(), inv_fastSquareRoot(), inv_inverseSqrt(). +* Finds the range of the argument, determines the optimal number of Newton-Raphson +* iterations and . Corresponding square root of the power of two is returned. +* Restrictions: Number is represented as Q1.30. +* Number is betweeen the range 2<x<=0 +* @param[in] x, length 1 +* @param[out] pow, length 1 +* @return # of NR iterations, x0 scaled between log(2) and log(4) and 2^N scaling (N=pow) +*/ +int test_limits_and_scale(long *x0, int *pow) +{ + long lowerlimit, upperlimit, oneiterlothr, oneiterhithr, zeroiterlothr, zeroiterhithr; + + // Lower Limit: ll = int32(log(2)*2^30); + lowerlimit = 744261118L; + //Upper Limit ul = int32(log(4)*2^30); + upperlimit = 1488522236L; + // int32(0.9*2^30) + oneiterlothr = 966367642L; + // int32(1.1*2^30) + oneiterhithr = 1181116006L; + // int32(0.99*2^30) + zeroiterlothr=1063004406L; + //int32(1.01*2^30) + zeroiterhithr=1084479242L; + + // Scale number such that Newton Raphson iteration works best: + // Find the power of two scaling that leaves the number in the optimal range, + // ll <= number <= ul. Note odd powers have special scaling further below + if (*x0 > upperlimit) { + // Halving the number will push it in the optimal range since largest value is 2 + *x0 = *x0>>1; + *pow=-1; + } else if (*x0 < lowerlimit) { + // Find position of highest bit, counting from left, and scale number + *pow=get_highest_bit_position((unsigned long*)x0); + if (*x0 >= upperlimit) { + // Halving the number will push it in the optimal range + *x0 = *x0>>1; + *pow=*pow-1; + } + else if (*x0 < lowerlimit) { + // Doubling the number will push it in the optimal range + *x0 = *x0<<1; + *pow=*pow+1; + } + } else { + *pow = 0; + } + + if ( *x0<oneiterlothr || *x0>oneiterhithr ) + return 3; // 3 NR iterations + if ( *x0<zeroiterlothr || *x0>zeroiterhithr ) + return 2; // 2 NR iteration + + return 1; // 1 NR iteration +} + +/** Auxiliary function used by testLimitsAndScale() +* Find the highest nonzero bit in an unsigned 32 bit integer: +* @param[in] value, length 1. +* @return highes bit position. +**/int get_highest_bit_position(unsigned long *value) +{ + int position; + position = 0; + if (*value == 0) return 0; + + if ((*value & 0xFFFF0000) == 0) { + position += 16; + *value=*value<<16; + } + if ((*value & 0xFF000000) == 0) { + position += 8; + *value=*value<<8; + } + if ((*value & 0xF0000000) == 0) { + position += 4; + *value=*value<<4; + } + if ((*value & 0xC0000000) == 0) { + position += 2; + *value=*value<<2; + } + + // If we got too far into sign bit, shift back. Note we are using an + // unsigned long here, so right shift is going to shift all the bits. + if ((*value & 0x80000000)) { + position -= 1; + *value=*value>>1; + } + return position; +} + +/* compute real part of quaternion, element[0] +@param[in] inQuat, 3 elements gyro quaternion +@param[out] outquat, 4 elements gyro quaternion +*/ +int inv_compute_scalar_part(const long * inQuat, long* outQuat) +{ + long scalarPart = 0; + + scalarPart = inv_fast_sqrt((1L<<30) - inv_q30_mult(inQuat[0], inQuat[0]) + - inv_q30_mult(inQuat[1], inQuat[1]) + - inv_q30_mult(inQuat[2], inQuat[2]) ); + outQuat[0] = scalarPart; + outQuat[1] = inQuat[0]; + outQuat[2] = inQuat[1]; + outQuat[3] = inQuat[2]; + + return 0; +} +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/ml_math_func.h b/6515/libsensors_iio/software/core/mllite/ml_math_func.h new file mode 100644 index 0000000..1540254 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/ml_math_func.h @@ -0,0 +1,129 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INVENSENSE_INV_MATH_FUNC_H__ +#define INVENSENSE_INV_MATH_FUNC_H__ + +#include "mltypes.h" + +#define GYRO_MAG_SQR_SHIFT 6 +#define NUM_ROTATION_MATRIX_ELEMENTS (9) +#define ROT_MATRIX_SCALE_LONG (1073741824L) +#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) +#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ + ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) +#define SIGNM(k)((int)(k)&1?-1:1) +#define SIGNSET(x) ((x) ? -1 : +1) + +#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f + +#ifdef __cplusplus +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)); + } + + static inline double inv_q30_to_double(long q30) + { + return (double) q30 / ((double)(1L << 30)); + } + + static inline float inv_q16_to_float(long q16) + { + return (float) q16 / (1L << 16); + } + + static inline double inv_q16_to_double(long q16) + { + return (double) q16 / (1L << 16); + } + + + + + long inv_q29_mult(long a, long b); + long inv_q30_mult(long a, long b); + + /* UMPL_ELIMINATE_64BIT Notes: + * An alternate implementation using float instead of long long accudoublemulators + * is provided for q29_mult and q30_mult. + * When long long accumulators are used and an alternate implementation is not + * available, we eliminate the entire function and header with a macro. + */ +#ifndef UMPL_ELIMINATE_64BIT + long inv_q30_div(long a, long b); + long inv_q_shift_mult(long a, long b, int shift); +#endif + + void inv_q_mult(const long *q1, const long *q2, long *qProd); + void inv_q_add(long *q1, long *q2, long *qSum); + void inv_q_normalize(long *q); + void inv_q_invert(const long *q, long *qInverted); + void inv_q_multf(const float *q1, const float *q2, float *qProd); + void inv_q_addf(const float *q1, const float *q2, float *qSum); + void inv_q_normalizef(float *q); + void inv_q_norm4(float *q); + void inv_q_invertf(const float *q, float *qInverted); + void inv_quaternion_to_rotation(const long *quat, long *rot); + unsigned char *inv_int32_to_big8(long x, unsigned char *big8); + long inv_big8_to_int32(const unsigned char *big8); + short inv_big8_to_int16(const unsigned char *big8); + short inv_little8_to_int16(const unsigned char *little8); + unsigned char *inv_int16_to_big8(short x, unsigned char *big8); + float inv_matrix_det(float *p, int *n); + void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); + double inv_matrix_detd(double *p, int *n); + void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); + float inv_wrap_angle(float ang); + float inv_angle_diff(float ang1, float ang2); + void inv_quaternion_to_rotation_vector(const long *quat, long *rot); + unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); + void inv_convert_to_body(unsigned short orientation, const long *input, long *output); + void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); + void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); + void inv_q_rotate(const long *q, const long *in, long *out); + void inv_vector_normalize(long *vec, int length); + uint32_t inv_checksum(const unsigned char *str, int len); + float inv_compass_angle(const long *compass, const long *grav, + const float *quat); + unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); + + static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) + { + return (long)((t1 - t2) / 1000000L); + } + + 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]); + + void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut); + + long inv_inverse_sqrt(long x0, int*rempow); + long inv_fast_sqrt(long x0); + long inv_one_over_x(long x0, int*pow); + int test_limits_and_scale(long *x0, int *pow); + int get_highest_bit_position(unsigned long *value); + int inv_compute_scalar_part(const long * inQuat, long* outQuat); + +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/6515/libsensors_iio/software/core/mllite/mpl.c b/6515/libsensors_iio/software/core/mllite/mpl.c new file mode 100644 index 0000000..0aa2242 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/mpl.c @@ -0,0 +1,77 @@ +/* + $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" +#include "message_layer.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()); + + // Get any left over messages and clear them. Throw message away as it is not + // initialized. + (void)inv_get_message_level_0(1); + + return INV_SUCCESS; +} + +const char ml_ver[] = "InvenSense MA 5.2.0 K"; + +/** + * @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/6515/libsensors_iio/software/core/mllite/mpl.h b/6515/libsensors_iio/software/core/mllite/mpl.h new file mode 100644 index 0000000..a6b5ac7 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/mpl.h @@ -0,0 +1,24 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_MPL_H__ +#define INV_MPL_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_init_mpl(void); +inv_error_t inv_start_mpl(void); +inv_error_t inv_get_version(char **version); + +#ifdef __cplusplus +} +#endif + +#endif // INV_MPL_H__ diff --git a/6515/libsensors_iio/software/core/mllite/results_holder.c b/6515/libsensors_iio/software/core/mllite/results_holder.c new file mode 100644 index 0000000..c59eb94 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/results_holder.c @@ -0,0 +1,893 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+/**
+ * @defgroup Results_Holder results_holder
+ * @brief Motion Library - Results Holder
+ * Holds the data for MPL
+ *
+ * @{
+ * @file results_holder.c
+ * @brief Results Holder for HAL.
+ */
+
+#include <string.h>
+
+#include "results_holder.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "message_layer.h"
+#include "log.h"
+
+struct results_t {
+ float nav_quat[4];
+ float game_quat[4];
+ long gam_quat[4];
+ float geomag_quat[4];
+ long accel_quat[4];
+ inv_time_t nav_timestamp;
+ inv_time_t gam_timestamp;
+ inv_time_t geomag_timestamp;
+ 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 */
+ long geomag_compass_correction[4]; /**< quaternion going from accel quaternion to geomag sensor fusion */
+ 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 */
+ long compass_count; /**< compass state internal counter */
+ int got_compass_bias; /**< Flag describing if compass bias is known */
+ int large_mag_field; /**< Flag describing if there is a large magnetic field */
+ int compass_state; /**< Internal compass state */
+ long status;
+ struct inv_sensor_cal_t *sensor;
+ float quat_confidence_interval;
+ float geo_mag_confidence_interval;
+ struct local_field_t mag_local_field;
+ struct local_field_t mpl_compass_cal;
+ int quat_validity;
+#ifdef WIN32
+ long last_quat[4];
+#endif
+};
+static struct results_t rh;
+
+/** @internal
+* Store a quaternion more suitable for gaming. This quaternion is often determined
+* using only gyro and accel.
+* @param[in] quat Length 4, Quaternion scaled by 2^30
+* @param[in] timestamp Timestamp of the 6-axis quaternion
+*/
+void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp)
+{
+ rh.status |= INV_6_AXIS_QUAT_SET;
+ memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat));
+ rh.gam_timestamp = timestamp;
+}
+
+/** @internal
+* Store a 9-axis quaternion.
+* @param[in] quat Length 4 in floating-point numbers
+* @param[in] timestamp Timestamp of the 9-axis quaternion
+*/
+void inv_store_nav_quaternion(const float *quat, inv_time_t timestamp)
+{
+ memcpy(&rh.nav_quat, quat, sizeof(rh.nav_quat));
+ rh.nav_timestamp = timestamp;
+}
+
+/** @internal
+* Store a 6-axis geomagnetic quaternion.
+* @param[in] quat Length 4 in floating-point numbers
+* @param[in] timestamp Timestamp of the geomag quaternion
+*/
+void inv_store_geomag_quaternion(const float *quat, inv_time_t timestamp)
+{
+ memcpy(&rh.geomag_quat, quat, sizeof(rh.geomag_quat));
+ rh.geomag_timestamp = timestamp;
+}
+
+/** @internal
+* Store a floating-point quaternion more suitable for gaming.
+* @param[in] quat Length 4 in floating-point numbers
+* @param[in] timestamp Timestamp of the 6-axis quaternion
+*/
+void inv_store_game_quaternion(const float *quat, inv_time_t timestamp)
+{
+ rh.status |= INV_6_AXIS_QUAT_SET;
+ memcpy(&rh.game_quat, quat, sizeof(rh.game_quat));
+ rh.gam_timestamp = timestamp;
+}
+
+/** @internal
+* Store a quaternion computed from accelerometer correction. This quaternion is
+* determined * using only accel, and used for geomagnetic fusion.
+* @param[in] quat Length 4, Quaternion scaled by 2^30
+*/
+void inv_store_accel_quaternion(const long *quat, inv_time_t timestamp)
+{
+ // rh.status |= INV_6_AXIS_QUAT_SET;
+ memcpy(&rh.accel_quat, quat, sizeof(rh.accel_quat));
+ rh.geomag_timestamp = timestamp;
+}
+/** @internal
+* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
+* @param[in] data Quaternion Adjustment
+* @param[in] timestamp Timestamp of when this is valid
+*/
+void inv_set_compass_correction(const long *data, inv_time_t timestamp)
+{
+ rh.status |= INV_COMPASS_CORRECTION_SET;
+ memcpy(rh.compass_correction, data, sizeof(rh.compass_correction));
+ rh.nav_timestamp = timestamp;
+}
+
+/** @internal
+* Sets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion.
+* @param[in] data Quaternion Adjustment
+* @param[in] timestamp Timestamp of when this is valid
+*/
+void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp)
+{
+ rh.status |= INV_GEOMAGNETIC_CORRECTION_SET;
+ memcpy(rh.geomag_compass_correction, data, sizeof(rh.geomag_compass_correction));
+ rh.geomag_timestamp = timestamp;
+}
+
+/** @internal
+* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
+* @param[out] data Quaternion Adjustment
+* @param[out] timestamp Timestamp of when this is valid
+*/
+void inv_get_compass_correction(long *data, inv_time_t *timestamp)
+{
+ memcpy(data, rh.compass_correction, sizeof(rh.compass_correction));
+ *timestamp = rh.nav_timestamp;
+}
+
+/** @internal
+* Gets the quaternion adjustment from 3 axis (accel) to 6 axis (with compass) quaternion.
+* @param[out] data Quaternion Adjustment
+* @param[out] timestamp Timestamp of when this is valid
+*/
+void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp)
+{
+ memcpy(data, rh.geomag_compass_correction, sizeof(rh.geomag_compass_correction));
+ *timestamp = rh.geomag_timestamp;
+}
+
+/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable.
+ * @return Returns non-zero if there is a large magnetic field.
+ */
+int inv_get_large_mag_field()
+{
+ return rh.large_mag_field;
+}
+
+/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable.
+ * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large.
+ */
+void inv_set_large_mag_field(int state)
+{
+ rh.large_mag_field = state;
+}
+
+/** Gets the accel state set by inv_set_acc_state()
+ * @return accel state.
+ */
+int inv_get_acc_state()
+{
+ return rh.acc_state;
+}
+
+/** Sets the accel state. See inv_get_acc_state() to get the value.
+ * @param[in] state value to set accel state to.
+ */
+void inv_set_acc_state(int state)
+{
+ rh.acc_state = state;
+ return;
+}
+
+/** Returns the motion state
+* @param[out] cntr Number of previous times a no motion event has occured in a row.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+int inv_get_motion_state(unsigned int *cntr)
+{
+ *cntr = rh.motion_state_counter;
+ return rh.motion_state;
+}
+
+/** Sets the motion state
+ * @param[in] state motion state where INV_NO_MOTION is not moving
+ * and INV_MOTION is moving.
+ */
+void inv_set_motion_state(unsigned char state)
+{
+ long set;
+ if (state == rh.motion_state) {
+ if (state == INV_NO_MOTION) {
+ rh.motion_state_counter++;
+ } else {
+ rh.motion_state_counter = 0;
+ }
+ return;
+ }
+ rh.motion_state_counter = 0;
+ rh.motion_state = state;
+ /* Equivalent to set = state, but #define's may change. */
+ if (state == INV_MOTION)
+ set = INV_MSG_MOTION_EVENT;
+ else
+ set = INV_MSG_NO_MOTION_EVENT;
+ inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0);
+}
+
+/** Sets the compass sensitivity
+ * @param[in] data Length 3, sensitivity for each compass axis
+ * scaled such that 1.0 = 2^30.
+ */
+void inv_set_mag_scale(const long *data)
+{
+ memcpy(rh.mag_scale, data, sizeof(rh.mag_scale));
+}
+
+/** Gets the compass sensitivity
+ * @param[out] data Length 3, sensitivity for each compass axis
+ * scaled such that 1.0 = 2^30.
+ */
+void inv_get_mag_scale(long *data)
+{
+ memcpy(data, rh.mag_scale, sizeof(rh.mag_scale));
+}
+
+/** Gets gravity vector
+ * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_gravity(long *data)
+{
+ long ldata[4];
+ inv_error_t result = inv_get_quaternion(ldata);
+
+ data[0] =
+ inv_q29_mult(ldata[1], ldata[3]) - inv_q29_mult(ldata[2], ldata[0]);
+ data[1] =
+ inv_q29_mult(ldata[2], ldata[3]) + inv_q29_mult(ldata[1], ldata[0]);
+ data[2] =
+ (inv_q29_mult(ldata[3], ldata[3]) + inv_q29_mult(ldata[0], ldata[0])) -
+ 1073741824L;
+
+ return result;
+}
+
+/** Returns a quaternion based only on accel.
+ * @param[out] data 3-axis accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_accel_quaternion(long *data)
+{
+ memcpy(data, rh.accel_quat, sizeof(rh.accel_quat));
+ return INV_SUCCESS;
+}
+inv_error_t inv_get_gravity_6x(long *data)
+{
+ data[0] =
+ inv_q29_mult(rh.gam_quat[1], rh.gam_quat[3]) - inv_q29_mult(rh.gam_quat[2], rh.gam_quat[0]);
+ data[1] =
+ inv_q29_mult(rh.gam_quat[2], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[1], rh.gam_quat[0]);
+ data[2] =
+ (inv_q29_mult(rh.gam_quat[3], rh.gam_quat[3]) + inv_q29_mult(rh.gam_quat[0], rh.gam_quat[0])) -
+ 1073741824L;
+ return INV_SUCCESS;
+}
+/** Returns a quaternion based only on gyro and accel.
+ * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp)
+{
+ data[0] = (long)MIN(MAX(rh.game_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[1] = (long)MIN(MAX(rh.game_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[2] = (long)MIN(MAX(rh.game_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[3] = (long)MIN(MAX(rh.game_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ *timestamp = rh.gam_timestamp;
+ return INV_SUCCESS;
+}
+
+/** Returns a quaternion.
+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_quaternion(long *data)
+{
+ data[0] = (long)MIN(MAX(rh.nav_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[1] = (long)MIN(MAX(rh.nav_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[2] = (long)MIN(MAX(rh.nav_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[3] = (long)MIN(MAX(rh.nav_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+
+ return INV_SUCCESS;
+}
+
+#ifdef WIN32
+/** Returns the last 9-axis quaternion genearted by MPL, so that
+ * it can be written to the DMP.
+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_last_quaternion(long *data)
+{
+ memcpy(data, rh.last_quat, sizeof(rh.last_quat));
+ return INV_SUCCESS;
+}
+
+/** Saves the last 9-axis quaternion generated by MPL.
+ * @param[in] data 9-axis quaternion data.
+ */
+inv_error_t inv_set_last_quaternion(long *data)
+{
+ memcpy(rh.last_quat, data, sizeof(rh.last_quat));
+ return INV_SUCCESS;
+}
+#endif
+
+/** Returns the status of the result holder.
+ * @param[out] rh_status Result holder status.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_result_holder_status(long *rh_status)
+{
+ *rh_status = rh.status;
+ return INV_SUCCESS;
+}
+
+/** Set the status of the result holder.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_set_result_holder_status(long rh_status)
+{
+ rh.status = rh_status;
+ return INV_SUCCESS;
+}
+
+/** Returns the status of the authenticity of the quaternion data.
+ * @param[out] value Authenticity of the quaternion data.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_quaternion_validity(int *value)
+{
+ *value = rh.quat_validity;
+ return INV_SUCCESS;
+}
+
+/** Set the status of the authenticity of the quaternion data.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_set_quaternion_validity(int value)
+{
+ rh.quat_validity = value;
+ return INV_SUCCESS;
+}
+
+/** Returns a quaternion based only on compass and accel.
+ * @param[out] data 6-axis compass and accel quaternion scaled such that 1.0 = 2^30.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp)
+{
+ data[0] = (long)MIN(MAX(rh.geomag_quat[0] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[1] = (long)MIN(MAX(rh.geomag_quat[1] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[2] = (long)MIN(MAX(rh.geomag_quat[2] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ data[3] = (long)MIN(MAX(rh.geomag_quat[3] * ((float)(1L << 30)), -2147483648.), 2147483647.);
+ *timestamp = rh.geomag_timestamp;
+ return INV_SUCCESS;
+}
+
+/** Returns a quaternion.
+ * @param[out] data 9-axis quaternion.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_quaternion_float(float *data)
+{
+ memcpy(data, rh.nav_quat, sizeof(rh.nav_quat));
+ return INV_SUCCESS;
+}
+
+/** Returns a quaternion based only on gyro and accel.
+ * @param[out] data 6-axis gyro and accel quaternion.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_6axis_quaternion_float(float *data, inv_time_t *timestamp)
+{
+ memcpy(data, rh.game_quat, sizeof(rh.game_quat));
+ *timestamp = rh.gam_timestamp;
+ return INV_SUCCESS;
+}
+
+/** Returns a quaternion based only on compass and accel.
+ * @param[out] data 6-axis compass and accel quaternion.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_get_geomagnetic_quaternion_float(float *data, inv_time_t *timestamp)
+{
+ memcpy(data, rh.geomag_quat, sizeof(rh.geomag_quat));
+ *timestamp = rh.geomag_timestamp;
+ return INV_SUCCESS;
+}
+
+/** Returns a quaternion with accuracy and timestamp.
+ * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
+ * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate.
+ * @param[out] timestamp Timestamp of this quaternion in nanoseconds
+ */
+void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp)
+{
+ inv_get_quaternion(data);
+ *timestamp = inv_get_last_timestamp();
+ if (inv_get_compass_on()) {
+ *accuracy = inv_get_mag_accuracy();
+ } else if (inv_get_gyro_on()) {
+ *accuracy = inv_get_gyro_accuracy();
+ }else if (inv_get_accel_on()) {
+ *accuracy = inv_get_accel_accuracy();
+ } else {
+ *accuracy = 0;
+ }
+}
+
+/** Callback that gets called everytime there is new data. It is
+ * registered by inv_start_results_holder().
+ * @param[in] sensor_cal New sensor data to process.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal)
+{
+ rh.sensor = sensor_cal;
+ return INV_SUCCESS;
+}
+
+/** Function to turn on this module. This is automatically called by
+ * inv_enable_results_holder(). Typically not called by users.
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_start_results_holder(void)
+{
+ inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER,
+ INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
+ return INV_SUCCESS;
+}
+
+/** Initializes results holder. This is called automatically by the
+* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_results_holder(void)
+{
+ memset(&rh, 0, sizeof(rh));
+ rh.mag_scale[0] = 1L<<30;
+ rh.mag_scale[1] = 1L<<30;
+ rh.mag_scale[2] = 1L<<30;
+ rh.compass_correction[0] = 1L<<30;
+ rh.gam_quat[0] = 1L<<30;
+ rh.nav_quat[0] = 1.;
+ rh.geomag_quat[0] = 1.;
+ rh.game_quat[0] = 1.;
+ rh.accel_quat[0] = 1L<<30;
+ rh.geomag_compass_correction[0] = 1L<<30;
+ rh.quat_confidence_interval = (float)M_PI;
+#ifdef WIN32
+ rh.last_quat[0] = 1L<<30;
+#endif
+
+
+#ifdef TEST
+ {
+ struct local_field_t local_field;
+ local_field.intensity = 48.0f; // uT
+ local_field.inclination = 60.0f; // degree
+ local_field.declination = 0.0f; // degree
+ inv_set_earth_magnetic_local_field_parameter(&local_field);
+ // inv_set_local_field_status(LOCAL_FILED_NOT_SET_BY_USER);
+ inv_set_local_field_status(LOCAL_FILED_SET_BY_USER);
+ inv_set_local_magnetic_field(48.0f, 59.0f, 0.0f);
+
+ // set default mpl calibration result for testing
+ local_field.intensity = 50.0f; // uT
+ local_field.inclination = 59.0f; // degree
+ local_field.declination = 0.0f; // degree
+ inv_set_mpl_magnetic_local_field_parameter(&local_field);
+ inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL);
+ }
+#endif
+
+ return INV_SUCCESS;
+}
+
+/** Turns on storage of results.
+*/
+inv_error_t inv_enable_results_holder()
+{
+ inv_error_t result;
+ result = inv_init_results_holder();
+ if ( result ) {
+ return result;
+ }
+
+ result = inv_register_mpl_start_notification(inv_start_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()
+ */
+int inv_got_compass_bias()
+{
+ return rh.got_compass_bias;
+}
+
+/** Sets whether we know the compass bias
+ * @param[in] state Set to 1 if we know the compass bias.
+ * Can be retrieved with inv_got_compass_bias()
+ */
+void inv_set_compass_bias_found(int state)
+{
+ rh.got_compass_bias = state;
+}
+
+/** Sets the compass state.
+ * @param[in] state Compass state. It can be retrieved with inv_get_compass_state().
+ */
+void inv_set_compass_state(int state)
+{
+ rh.compass_state = state;
+}
+
+/** Get's the compass state
+ * @return the compass state that was set with inv_set_compass_state()
+ */
+int inv_get_compass_state()
+{
+ return rh.compass_state;
+}
+
+/** Set compass bias error. See inv_get_compass_bias_error()
+ * @param[in] bias_error Set's how accurate we know the compass bias. It is the
+ * error squared.
+ */
+void inv_set_compass_bias_error(const long *bias_error)
+{
+ memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error));
+}
+
+/** Get's compass bias error. See inv_set_compass_bias_error() for setting.
+ * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared.
+ */
+void inv_get_compass_bias_error(long *bias_error)
+{
+ memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error));
+}
+
+/**
+ * @brief Returns 3-element vector of accelerometer data in body frame
+ * with gravity removed
+ * @param[out] data 3-element vector of accelerometer data in body frame
+ * with gravity removed
+ * @return INV_SUCCESS if successful
+ * INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_linear_accel(long *data)
+{
+ long gravity[3];
+
+ if (data != NULL)
+ {
+ inv_get_accel_set(data, NULL, NULL);
+ inv_get_gravity(gravity);
+ data[0] -= gravity[0] >> 14;
+ data[1] -= gravity[1] >> 14;
+ data[2] -= gravity[2] >> 14;
+ return INV_SUCCESS;
+ }
+ else {
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+}
+
+/**
+ * @brief Returns 3-element vector of accelerometer data in body frame
+ * @param[out] data 3-element vector of accelerometer data in body frame
+ * @return INV_SUCCESS if successful
+ * INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_accel(long *data)
+{
+ if (data != NULL) {
+ inv_get_accel_set(data, NULL, NULL);
+ return INV_SUCCESS;
+ }
+ else {
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+}
+
+/**
+ * @brief Returns 3-element vector of accelerometer float data
+ * @param[out] data 3-element vector of accelerometer float data
+ * @return INV_SUCCESS if successful
+ * INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_accel_float(float *data)
+{
+ long tdata[3];
+ unsigned char i;
+
+ if (data != NULL && !inv_get_accel(tdata)) {
+ for (i = 0; i < 3; ++i) {
+ data[i] = ((float)tdata[i] / (1L << 16));
+ }
+ return INV_SUCCESS;
+ }
+ else {
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+}
+
+/**
+ * @brief Returns 3-element vector of gyro float data
+ * @param[out] data 3-element vector of gyro float data
+ * @return INV_SUCCESS if successful
+ * INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_gyro_float(float *data)
+{
+ long tdata[3];
+ unsigned char i;
+
+ if (data != NULL) {
+ inv_get_gyro_set(tdata, NULL, NULL);
+ for (i = 0; i < 3; ++i) {
+ data[i] = ((float)tdata[i] / (1L << 16));
+ }
+ return INV_SUCCESS;
+ }
+ else {
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+}
+
+/** Set 9 axis 95% heading confidence interval for quaternion
+* @param[in] ci Confidence interval in radians.
+*/
+void inv_set_heading_confidence_interval(float ci)
+{
+ rh.quat_confidence_interval = ci;
+}
+
+/** Get 9 axis 95% heading confidence interval for quaternion
+* @return Confidence interval in radians.
+*/
+float inv_get_heading_confidence_interval(void)
+{
+ return rh.quat_confidence_interval;
+}
+
+/** Set 6 axis (accel and compass) 95% heading confidence interval for quaternion
+* @param[in] ci Confidence interval in radians.
+*/
+void inv_set_accel_compass_confidence_interval(float ci)
+{
+ rh.geo_mag_confidence_interval = ci;
+}
+
+/** Get 6 axis (accel and compass) 95% heading confidence interval for quaternion
+* @return Confidence interval in radians.
+*/
+float inv_get_accel_compass_confidence_interval(void)
+{
+ return rh.geo_mag_confidence_interval;
+}
+
+/**
+ * @brief Returns 3-element vector of linear accel float data
+ * @param[out] data 3-element vector of linear aceel float data
+ * @return INV_SUCCESS if successful
+ * INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_get_linear_accel_float(float *data)
+{
+ long tdata[3];
+ unsigned char i;
+
+ if (data != NULL && !inv_get_linear_accel(tdata)) {
+ for (i = 0; i < 3; ++i) {
+ data[i] = ((float)tdata[i] / (1L << 16));
+ }
+ return INV_SUCCESS;
+ }
+ else {
+ return INV_ERROR_INVALID_PARAMETER;
+ }
+}
+
+/**
+ * @brief Returns the status of earth magnetic field local field parameters
+ * @param[out] N/A
+ * @return status of local field, defined in enum compass_local_field_e
+ */
+enum compass_local_field_e inv_get_local_field_status(void)
+{
+ return rh.mag_local_field.mpl_match_status;
+}
+
+/** Set the status of earth magnetic field local field parameters
+* @param[in] status of earth magnetic field local field parameters.
+*/
+void inv_set_local_field_status(enum compass_local_field_e status)
+{
+ rh.mag_local_field.mpl_match_status = status;
+}
+
+/** Set the parameters of earth magnetic field local field
+* @param[in] the earth magnetic field local field parameters.
+*/
+void inv_set_earth_magnetic_local_field_parameter(struct local_field_t *parameters)
+{
+ rh.mag_local_field.intensity = parameters->intensity; // radius
+ rh.mag_local_field.inclination = parameters->inclination; // dip angle
+ rh.mag_local_field.declination = parameters->declination; // yaw deviation angle from true north
+
+ inv_set_local_field_status(LOCAL_FILED_SET_BY_USER);
+}
+
+/**
+ * @brief Returns the parameters of earth magnetic field local field
+ * @param[out] the parameters of earth magnetic field local field
+ * @return N/A
+ */
+void inv_get_earth_magnetic_local_field_parameter(struct local_field_t *parameters)
+{
+ parameters->intensity = rh.mag_local_field.intensity; // radius
+ parameters->inclination = rh.mag_local_field.inclination; // dip angle
+ parameters->declination = rh.mag_local_field.declination; // yaw deviation angle from true north
+ parameters->mpl_match_status = rh.mag_local_field.mpl_match_status;
+}
+
+/**
+ * @brief Returns the status of mpl calibrated magnetic field local field parameters
+ * @param[out] N/A
+ * @return status of local field, defined in enum compass_local_field_e
+ */
+enum compass_local_field_e inv_get_mpl_mag_field_status(void)
+{
+ return rh.mpl_compass_cal.mpl_match_status;
+}
+
+/** Set the status of mpl calibrated magnetic field local field parameters
+* @param[in] status of earth magnetic field local field parameters.
+*/
+void inv_set_mpl_mag_field_status(enum compass_local_field_e status)
+{
+ rh.mpl_compass_cal.mpl_match_status = status;
+}
+
+/** Set the magnetic field local field struct object
+* @param[in] status of earth magnetic field local field parameters.
+*/
+inv_error_t inv_set_local_magnetic_field(float intensity, float inclination, float declination)
+{
+ struct local_field_t local_field;
+ local_field.intensity = intensity; // radius
+ local_field.inclination = inclination; // dip angle angle degree
+ local_field.declination = declination; // yaw deviation angle from true north, eastward as positive
+ local_field.mpl_match_status = LOCAL_FILED_SET_BY_USER;
+
+ inv_set_earth_magnetic_local_field_parameter(&local_field);
+ return 0;
+}
+
+/** Set the parameters of mpl calibrated magnetic field local field
+* This API is used by mpl only.
+* @param[in] the earth magnetic field local field parameters.
+ * @return INV_SUCCESS if successful
+ * INV_ERROR_INVALID_PARAMETER if invalid input pointer
+ */
+inv_error_t inv_set_mpl_magnetic_local_field_parameter(struct local_field_t *parameters)
+{
+ enum compass_local_field_e mpl_status;
+ struct local_field_t local_field;
+ inv_error_t status;
+
+ rh.mpl_compass_cal.intensity = parameters->intensity; // radius
+ rh.mpl_compass_cal.inclination = parameters->inclination; // dip angle
+ rh.mpl_compass_cal.declination = parameters->declination; // yaw deviation angle from true north
+
+ mpl_status = inv_get_mpl_mag_field_status();
+ inv_get_earth_magnetic_local_field_parameter(&local_field);
+
+ status = INV_SUCCESS;
+
+ switch (inv_get_local_field_status()) {
+ case LOCAL_FILED_NOT_SET_BY_USER:
+ if (mpl_status == LOCAL_FILED_NOT_SET_BY_USER) {
+ inv_set_mpl_mag_field_status(LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL);
+ } else {
+ // illegal status
+ status = INV_ERROR_INVALID_PARAMETER;
+ }
+ break;
+ case LOCAL_FILED_SET_BY_USER:
+ switch (mpl_status) {
+ case LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL:
+ case LOCAL_FIELD_SET_MATCH_WITH_MPL:
+ if ( (ABS(local_field.intensity - parameters->intensity) < 5.0f) &&
+ (ABS(local_field.intensity - parameters->intensity) < 5.0f) ) {
+ inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_MATCH_WITH_MPL);
+ } else {
+ inv_set_mpl_mag_field_status(LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL);
+ }
+ break;
+ case LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL:
+ // no status update
+ break;
+ case LOCAL_FILED_NOT_SET_BY_USER:
+ case LOCAL_FILED_SET_BY_USER:
+ default:
+ // illegal status
+ status = INV_ERROR_INVALID_PARAMETER;
+ break;
+ }
+ break;
+ case LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL:
+ case LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL:
+ case LOCAL_FIELD_SET_MATCH_WITH_MPL:
+ default:
+ // illegal status
+ status = INV_ERROR_INVALID_PARAMETER;
+ break;
+ }
+ return status;
+}
+
+/**
+ * @brief Returns the parameters of mpl calibrated magnetic field local field
+ * @param[out] the parameters of earth magnetic field local field
+ * @return N/A
+ */
+void inv_get_mpl_magnetic_local_field_parameter(struct local_field_t *parameters)
+{
+ parameters->intensity = rh.mpl_compass_cal.intensity; // radius
+ parameters->inclination = rh.mpl_compass_cal.inclination; // dip angle
+ parameters->declination = rh.mpl_compass_cal.declination; // yaw deviation angle from true north
+ parameters->mpl_match_status = rh.mpl_compass_cal.mpl_match_status;
+}
+
+/**
+ * @}
+ */
diff --git a/6515/libsensors_iio/software/core/mllite/results_holder.h b/6515/libsensors_iio/software/core/mllite/results_holder.h new file mode 100644 index 0000000..342080d --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/results_holder.h @@ -0,0 +1,144 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_RESULTS_HOLDER_H__ +#define INV_RESULTS_HOLDER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define INV_MOTION 0x0001 +#define INV_NO_MOTION 0x0002 + + + + /**************************************************************************/ + /* The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = */ + /* 2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even. */ + /* The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = */ + /* 2^ACC_MAG_SQR_SHIFT */ + /**************************************************************************/ +#define ACC_MAG_SQR_SHIFT 16 + +enum compass_local_field_e { + // status for user input earth magnetic local field + LOCAL_FILED_NOT_SET_BY_USER = 0, + LOCAL_FILED_SET_BY_USER = 1, + + // status for mpl calibrated based magnetical field + LOCAL_FILED_NOT_SET_BY_USER_BUT_SET_BY_MPL = 2, + LOCAL_FIELD_SET_BUT_NOT_MATCH_WITH_MPL = 3, + LOCAL_FIELD_SET_MATCH_WITH_MPL = 4, +}; + +struct local_field_t { + float intensity; // radius
+ float inclination; // dip angle angle degree
+ float declination; // yaw deviation angle from true north, eastward as positive + enum compass_local_field_e mpl_match_status; +}; + +// earth magnetic field access API +enum compass_local_field_e inv_get_local_field_status(void); +void inv_set_local_field_status(enum compass_local_field_e status); + +void inv_set_earth_magnetic_local_field_parameter(struct local_field_t *parameters); +void inv_get_earth_magnetic_local_field_parameter(struct local_field_t *parameters); + +// mpl calibrated magnetic field access API +enum compass_local_field_e inv_get_mpl_mag_field_status(void); +void inv_set_mpl_mag_field_status(enum compass_local_field_e status); + +inv_error_t inv_set_mpl_magnetic_local_field_parameter(struct local_field_t *parameters); +void inv_get_mpl_magnetic_local_field_parameter(struct local_field_t *parameters); + +// quaternion store API +void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp); +void inv_store_accel_quaternion(const long *quat, inv_time_t timestamp); +void inv_store_nav_quaternion(const float *quat, inv_time_t timestamp); +void inv_store_game_quaternion(const float *quat, inv_time_t timestamp); +void inv_store_geomag_quaternion(const float *quat, inv_time_t timestamp); + +// States +#define SF_NORMAL 0 +#define SF_UNCALIBRATED 1 +#define SF_STARTUP_SETTLE 2 +#define SF_FAST_SETTLE 3 +#define SF_DISTURBANCE 4 +#define SF_SLOW_SETTLE 5 + +// These 2 status bits are used to control when the 9 axis quaternion is updated +#define INV_COMPASS_CORRECTION_SET 1 +#define INV_6_AXIS_QUAT_SET 2 +#define INV_GEOMAGNETIC_CORRECTION_SET 4 + +int inv_get_acc_state(); +void inv_set_acc_state(int state); +int inv_get_motion_state(unsigned int *cntr); +void inv_set_motion_state(unsigned char state); +inv_error_t inv_get_gravity(long *data); +inv_error_t inv_get_gravity_6x(long *data); +inv_error_t inv_get_6axis_quaternion(long *data, inv_time_t *timestamp); +inv_error_t inv_get_quaternion(long *data); +inv_error_t inv_get_quaternion_float(float *data); +inv_error_t inv_get_6axis_quaternion_float(float *data, inv_time_t *timestamp); +inv_error_t inv_get_geomagnetic_quaternion_float(float *data, inv_time_t *timestamp); +#ifdef WIN32 +inv_error_t inv_get_last_quaternion(long *data); +inv_error_t inv_set_last_quaternion(long *data); +#endif +void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp); +inv_error_t inv_get_accel_quaternion(long *data); +inv_error_t inv_get_geomagnetic_quaternion(long *data, inv_time_t *timestamp); +void inv_set_geomagnetic_compass_correction(const long *data, inv_time_t timestamp); +void inv_get_geomagnetic_compass_correction(long *data, inv_time_t *timestamp); +inv_error_t inv_get_result_holder_status(long *rh_status); +inv_error_t inv_set_result_holder_status(long rh_status); +inv_error_t inv_get_quaternion_validity(int *value); +inv_error_t inv_set_quaternion_validity(int value); + +// set magnetic field by location +inv_error_t inv_set_local_magnetic_field(float intensity, float inclination, float declination); + +inv_error_t inv_enable_results_holder(); +inv_error_t inv_init_results_holder(void); + +/* Magnetic Field Parameters*/ +void inv_set_mag_scale(const long *data); +void inv_get_mag_scale(long *data); +void inv_set_compass_correction(const long *data, inv_time_t timestamp); +void inv_get_compass_correction(long *data, inv_time_t *timestamp); +int inv_got_compass_bias(); +void inv_set_compass_bias_found(int state); +int inv_get_large_mag_field(); +void inv_set_large_mag_field(int state); +void inv_set_compass_state(int state); +int inv_get_compass_state(); +void inv_set_compass_bias_error(const long *bias_error); +void inv_get_compass_bias_error(long *bias_error); +inv_error_t inv_get_linear_accel(long *data); +inv_error_t inv_get_accel(long *data); +inv_error_t inv_get_accel_float(float *data); +inv_error_t inv_get_gyro_float(float *data); +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); + +void inv_set_accel_compass_confidence_interval(float ci); +float inv_get_accel_compass_confidence_interval(void); + +int inv_got_accel_bias(); +void inv_set_accel_bias_found(int state); + + +#ifdef __cplusplus +} +#endif + +#endif // INV_RESULTS_HOLDER_H__ diff --git a/6515/libsensors_iio/software/core/mllite/start_manager.c b/6515/libsensors_iio/software/core/mllite/start_manager.c new file mode 100644 index 0000000..3e082d4 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/start_manager.c @@ -0,0 +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; +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/start_manager.h b/6515/libsensors_iio/software/core/mllite/start_manager.h new file mode 100644 index 0000000..899e3f5 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/start_manager.h @@ -0,0 +1,27 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_START_MANAGER_H__ +#define INV_START_MANAGER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" + +/** Max number of start callbacks we can handle. */ +#define INV_MAX_START_CB 20 + +inv_error_t inv_init_start_manager(void); +inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void)); +inv_error_t inv_execute_mpl_start_notification(void); +inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void)); + +#ifdef __cplusplus +} +#endif +#endif // INV_START_MANAGER_H__ diff --git a/6515/libsensors_iio/software/core/mllite/storage_manager.c b/6515/libsensors_iio/software/core/mllite/storage_manager.c new file mode 100644 index 0000000..419b9d0 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/storage_manager.c @@ -0,0 +1,210 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/** + * @defgroup Storage_Manager storage_manager + * @brief Motion Library - Stores Data for functions. + * + * + * @{ + * @file storage_manager.c + * @brief Load and Store Manager. + */ +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MLLITE" + +#include <string.h> + +#include "storage_manager.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" + +/* Must be changed if the format of storage changes */ +#define DEFAULT_KEY 29681 + +typedef inv_error_t (*load_func_t)(const unsigned char *data); +typedef inv_error_t (*save_func_t)(unsigned char *data); +/** Max number of entites that can be stored */ +#define NUM_STORAGE_BOXES 20 + +struct data_header_t { + long size; + uint32_t checksum; + unsigned int key; +}; + +struct data_storage_t { + int num; /**< Number of differnt save entities */ + size_t total_size; /**< Size in bytes to store non volatile data */ + load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ + save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ + struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ +}; +static struct data_storage_t ds; + +/** Should be called once before using any of the storage methods. Typically +* called first by inv_init_mpl().*/ +void inv_init_storage_manager() +{ + memset(&ds, 0, sizeof(ds)); + ds.total_size = sizeof(struct data_header_t); +} + +/** Used to register your mechanism to load and store non-volative data. This should typical be +* called during the enable function for your feature. +* @param[in] load_func function pointer you will use to receive data that was stored for you. +* @param[in] save_func function pointer you will use to save any data you want saved to +* non-volatile memory between runs. +* @param[in] size The size in bytes of the amount of data you want loaded and saved. +* @param[in] key The key associated with your data type should be unique across MPL. +* The key should change when your type of data for storage changes. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), + inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) +{ + int kk; + // Check if this has been registered already + for (kk=0; kk<ds.num; ++kk) { + if (key == ds.hd[kk].key) { + return INV_ERROR_INVALID_PARAMETER; + } + } + // Make sure there is room + if (ds.num >= NUM_STORAGE_BOXES) { + return INV_ERROR_INVALID_PARAMETER; + } + // Add to list + ds.hd[ds.num].key = key; + ds.hd[ds.num].size = size; + ds.load[ds.num] = load_func; + ds.save[ds.num] = save_func; + ds.total_size += size + sizeof(struct data_header_t); + ds.num++; + + return INV_SUCCESS; +} + +/** Returns the memory size needed to perform a store +* @param[out] size Size in bytes of memory needed to store. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_get_mpl_state_size(size_t *size) +{ + *size = ds.total_size; + return INV_SUCCESS; +} + +/** @internal + * Finds key in ds.hd[] array and returns location + * @return location where key exists in array, -1 if not found. + */ +static int inv_find_entry(unsigned int key) +{ + int kk; + for (kk=0; kk<ds.num; ++kk) { + if (key == ds.hd[kk].key) { + return kk; + } + } + return -1; +} + +/** This function takes a block of data that has been saved in non-volatile memory and pushes +* to the proper locations. Multiple error checks are performed on the data. +* @param[in] data Data that was saved to be loaded up by MPL +* @param[in] length Length of data vector in bytes +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) +{ + struct data_header_t *hd; + int entry; + uint32_t checksum; + long len; + + len = length; // Important so we get negative numbers + if (data == NULL || len == 0) + return INV_SUCCESS; + if (len < sizeof(struct data_header_t)) + return INV_ERROR_CALIBRATION_LOAD; // No data + hd = (struct data_header_t *)data; + if (hd->key != DEFAULT_KEY) + return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption + len = MIN(hd->size, len); + len = hd->size; + len -= sizeof(struct data_header_t); + data += sizeof(struct data_header_t); + checksum = inv_checksum(data, len); + if (checksum != hd->checksum) + return INV_ERROR_CALIBRATION_LOAD; // Data corruption + + while (len > (long)sizeof(struct data_header_t)) { + hd = (struct data_header_t *)data; + entry = inv_find_entry(hd->key); + data += sizeof(struct data_header_t); + len -= sizeof(struct data_header_t); + if (entry >= 0 && len >= hd->size) { + if (hd->size != ds.hd[entry].size) + return INV_ERROR_CALIBRATION_LEN; + checksum = inv_checksum(data, hd->size); + if (checksum != hd->checksum) + return INV_ERROR_CALIBRATION_LOAD; + ds.load[entry](data); + } + len -= hd->size; + if (len >= 0) + data = data + hd->size; + } + + return INV_SUCCESS; +} + +/** This function fills up a block of memory to be stored in non-volatile memory. +* @param[out] data Place to store data, size of sz, must be at least size +* returned by inv_get_mpl_state_size() +* @param[in] sz Size of data. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) +{ + unsigned char *cur; + int kk; + struct data_header_t *hd; + + if (data == NULL || sz == 0) + return INV_ERROR_CALIBRATION_LOAD; + if (sz >= ds.total_size) { + cur = data + sizeof(struct data_header_t); + for (kk = 0; kk < ds.num; ++kk) { + hd = (struct data_header_t *)cur; + cur += sizeof(struct data_header_t); + ds.save[kk](cur); + hd->checksum = inv_checksum(cur, ds.hd[kk].size); + hd->size = ds.hd[kk].size; + hd->key = ds.hd[kk].key; + cur += ds.hd[kk].size; + } + } else { + return INV_ERROR_CALIBRATION_LOAD; + } + + hd = (struct data_header_t *)data; + hd->checksum = inv_checksum(data + sizeof(struct data_header_t), + ds.total_size - sizeof(struct data_header_t)); + hd->key = DEFAULT_KEY; + hd->size = ds.total_size; + + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/core/mllite/storage_manager.h b/6515/libsensors_iio/software/core/mllite/storage_manager.h new file mode 100644 index 0000000..7255591 --- /dev/null +++ b/6515/libsensors_iio/software/core/mllite/storage_manager.h @@ -0,0 +1,30 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_STORAGE_MANAGER_H__ +#define INV_STORAGE_MANAGER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_register_load_store( + inv_error_t (*load_func)(const unsigned char *data), + inv_error_t (*save_func)(unsigned char *data), + size_t size, unsigned int key); +void inv_init_storage_manager(void); + +inv_error_t inv_get_mpl_state_size(size_t *size); +inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len); +inv_error_t inv_save_mpl_states(unsigned char *data, size_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* INV_STORAGE_MANAGER_H__ */ diff --git a/6515/libsensors_iio/software/core/mpl/accel_auto_cal.h b/6515/libsensors_iio/software/core/mpl/accel_auto_cal.h new file mode 100644 index 0000000..6641a7f --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/accel_auto_cal.h @@ -0,0 +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__ + diff --git a/6515/libsensors_iio/software/core/mpl/authenticate.h b/6515/libsensors_iio/software/core/mpl/authenticate.h new file mode 100644 index 0000000..df3e54d --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/authenticate.h @@ -0,0 +1,29 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id$ + * + ******************************************************************************/ + +#ifndef MLDMP_AUTHENTICATE_H__ +#define MLDMP_AUTHENTICATE_H__ + +#include "invensense.h" + +inv_error_t inv_check_key(void); + +#define INV_9AXIS_QUAT_VALID 0x10 +#define INV_9AXIS_QUAT_FALSE 0x20 +#define INV_6AXIS_QUAT_VALID 0x40 +#define INV_6AXIS_QUAT_FALSE 0x80 +#define INV_6AXIS_QUAT_NO_DMP 0x100 +#define INV_6AXIS_GEOMAG_QUAT_VALID 0x200 +#define INV_6AXIS_GEOMAG_QUAT_FALSE 0x400 + +#endif /* MLDMP_AUTHENTICATE_H__ */ diff --git a/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so Binary files differnew file mode 100755 index 0000000..e1b0276 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/build/android/libmplmpu.so diff --git a/6515/libsensors_iio/software/core/mpl/build/android/shared.mk b/6515/libsensors_iio/software/core/mpl/build/android/shared.mk new file mode 100644 index 0000000..1d6f904 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/build/android/shared.mk @@ -0,0 +1,90 @@ +MPL_LIB_NAME ?= mplmpu +LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc +STRIP ?= $(CROSS)strip -g + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +MPL_DIR = $(INV_ROOT)/software/core/mpl + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -shared +LFLAGS += -Wl,-soname,$(LIBRARY) +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 + +#################################################################################################### +## sources + +INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES, VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all mpl clean cleanall + +all: mpl + +mpl: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") + $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) + $(STRIP) -g $(LIBRARY) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/6515/libsensors_iio/software/core/mpl/build/filelist.mk b/6515/libsensors_iio/software/core/mpl/build/filelist.mk new file mode 100644 index 0000000..34ae4d1 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/build/filelist.mk @@ -0,0 +1,41 @@ +#### filelist.mk for mpl #### + +# headers for sources +HEADERS := $(MPL_DIR)/fast_no_motion.h +HEADERS += $(MPL_DIR)/fusion_9axis.h +HEADERS += $(MPL_DIR)/motion_no_motion.h +HEADERS += $(MPL_DIR)/no_gyro_fusion.h +HEADERS += $(MPL_DIR)/quaternion_supervisor.h +HEADERS += $(MPL_DIR)/gyro_tc.h +HEADERS += $(MPL_DIR)/authenticate.h +HEADERS += $(MPL_DIR)/accel_auto_cal.h +HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h +HEADERS += $(MPL_DIR)/compass_vec_cal.h +HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h +HEADERS += $(MPL_DIR)/mag_disturb.h +HEADERS += $(MPL_DIR)/mag_disturb_protected.h +HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h +HEADERS += $(MPL_DIR)/heading_from_gyro.h +HEADERS += $(MPL_DIR)/compass_fit.h +HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h + +# sources +SOURCES := $(MPL_DIR)/fast_no_motion.c +SOURCES += $(MPL_DIR)/fusion_9axis.c +SOURCES += $(MPL_DIR)/motion_no_motion.c +SOURCES += $(MPL_DIR)/no_gyro_fusion.c +SOURCES += $(MPL_DIR)/quaternion_supervisor.c +SOURCES += $(MPL_DIR)/gyro_tc.c +SOURCES += $(MPL_DIR)/authenticate.c +SOURCES += $(MPL_DIR)/accel_auto_cal.c +SOURCES += $(MPL_DIR)/compass_vec_cal.c +SOURCES += $(MPL_DIR)/mag_disturb.c +SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c +SOURCES += $(MPL_DIR)/heading_from_gyro.c +SOURCES += $(MPL_DIR)/compass_fit.c +SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c + +INV_SOURCES += $(SOURCES) + +VPATH = $(MPL_DIR) + diff --git a/6515/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/6515/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h new file mode 100644 index 0000000..7669b31 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h @@ -0,0 +1,31 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_COMPASSBIASWGYRO_H__ +#define MLDMP_COMPASSBIASWGYRO_H__ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_compass_bias_w_gyro(); + inv_error_t inv_disable_compass_bias_w_gyro(); + void inv_init_compass_bias_w_gyro(); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_COMPASSBIASWGYRO_H__ diff --git a/6515/libsensors_iio/software/core/mpl/compass_fit.h b/6515/libsensors_iio/software/core/mpl/compass_fit.h new file mode 100644 index 0000000..be3cce8 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/compass_fit.h @@ -0,0 +1,28 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +#ifndef INV_MLDMP_COMPASSFIT_H__ +#define INV_MLDMP_COMPASSFIT_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void inv_init_compass_fit(void); +inv_error_t inv_enable_compass_fit(void); +inv_error_t inv_disable_compass_fit(void); +inv_error_t inv_start_compass_fit(void); +inv_error_t inv_stop_compass_fit(void); + +#ifdef __cplusplus +} +#endif + + +#endif // INV_MLDMP_COMPASSFIT_H__ diff --git a/6515/libsensors_iio/software/core/mpl/compass_vec_cal.h b/6515/libsensors_iio/software/core/mpl/compass_vec_cal.h new file mode 100644 index 0000000..003a312 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/compass_vec_cal.h @@ -0,0 +1,36 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef COMPASS_ONLY_CAL_H__ +#define COMPASS_ONLY_CAL_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_vector_compass_cal(); +inv_error_t inv_disable_vector_compass_cal(); +inv_error_t inv_start_vector_compass_cal(void); +inv_error_t inv_stop_vector_compass_cal(void); +void inv_vector_compass_cal_sensitivity(float sens); +inv_error_t inv_init_vector_compass_cal(); + +inv_error_t inv_mcb_switch(int i); +#ifdef __cplusplus +} +#endif + +#endif // COMPASS_ONLY_CAL_H__ + diff --git a/6515/libsensors_iio/software/core/mpl/fast_no_motion.h b/6515/libsensors_iio/software/core/mpl/fast_no_motion.h new file mode 100644 index 0000000..cb05d3b --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/fast_no_motion.h @@ -0,0 +1,52 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_FAST_NO_MOTION_H__ +#define MLDMP_FAST_NO_MOTION_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_fast_nomot(void); + inv_error_t inv_disable_fast_nomot(void); + inv_error_t inv_start_fast_nomot(void); + inv_error_t inv_stop_fast_nomot(void); + inv_error_t inv_init_fast_nomot(void); + void inv_set_default_number_of_samples(int count); + 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); + + inv_time_t fast_nomot_get_gyro_bias_update_time(void); + void fast_nomot_set_gyro_bias_update_time(struct inv_sensor_cal_t *sensor_cal); + + int fast_nomot_get_gyro_calibration_confidence_level(struct inv_sensor_cal_t *sensor_cal); + void fast_nomot_set_gyro_calibration_confidence_level_time_threshold(float time_seconds); + + void inv_fnm_debug_print(void); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FAST_NO_MOTION_H__ + diff --git a/6515/libsensors_iio/software/core/mpl/fusion_9axis.h b/6515/libsensors_iio/software/core/mpl/fusion_9axis.h new file mode 100644 index 0000000..3a18519 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/fusion_9axis.h @@ -0,0 +1,39 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_FUSION9AXIS_H__ +#define MLDMP_FUSION9AXIS_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + void inv_init_9x_fusion(void); + inv_error_t inv_9x_fusion_state_change(unsigned char newState); + inv_error_t inv_enable_9x_sensor_fusion(void); + 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); + int inv_verify_9x_fusion_data(float *data); + + float inv_9x_sensor_fusion_get_correction_angle(void); +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FUSION9AXIS_H__ diff --git a/6515/libsensors_iio/software/core/mpl/gyro_tc.h b/6515/libsensors_iio/software/core/mpl/gyro_tc.h new file mode 100644 index 0000000..3347a14 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/gyro_tc.h @@ -0,0 +1,43 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef _GYRO_TC_H +#define _GYRO_TC_H_ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_gyro_tc(void); +inv_error_t inv_disable_gyro_tc(void); +inv_error_t inv_start_gyro_tc(void); +inv_error_t inv_stop_gyro_tc(void); + +inv_error_t inv_get_gyro_ts(long *data); +inv_error_t inv_set_gyro_ts(long *data); + +inv_error_t inv_init_gyro_ts(void); + +inv_error_t inv_set_gtc_max_temp(long data); +inv_error_t inv_set_gtc_min_temp(long data); + +inv_error_t inv_print_gtc_data(void); + +#ifdef __cplusplus +} +#endif + +#endif /* _GYRO_TC_H */ + diff --git a/6515/libsensors_iio/software/core/mpl/heading_from_gyro.h b/6515/libsensors_iio/software/core/mpl/heading_from_gyro.h new file mode 100644 index 0000000..09ecc42 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/heading_from_gyro.h @@ -0,0 +1,33 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef _HEADING_FROM_GYRO_H_ +#define _HEADING_FROM_GYRO_H_ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_heading_from_gyro(void); + inv_error_t inv_disable_heading_from_gyro(void); + void inv_init_heading_from_gyro(void); + inv_error_t inv_start_heading_from_gyro(void); + inv_error_t inv_stop_heading_from_gyro(void); + +#ifdef __cplusplus +} +#endif + + +#endif /* _HEADING_FROM_GYRO_H_ */ diff --git a/6515/libsensors_iio/software/core/mpl/inv_math.h b/6515/libsensors_iio/software/core/mpl/inv_math.h new file mode 100644 index 0000000..175511a --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/inv_math.h @@ -0,0 +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> diff --git a/6515/libsensors_iio/software/core/mpl/invensense_adv.h b/6515/libsensors_iio/software/core/mpl/invensense_adv.h new file mode 100644 index 0000000..12932c9 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/invensense_adv.h @@ -0,0 +1,31 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/* + Main header file for Invensense's Advanced library. +*/ + +#include "accel_auto_cal.h" +#include "compass_bias_w_gyro.h" +#include "compass_fit.h" +#include "compass_vec_cal.h" +#include "fast_no_motion.h" +#include "fusion_9axis.h" +#include "gyro_tc.h" +#include "heading_from_gyro.h" +#include "mag_disturb.h" +#include "motion_no_motion.h" +#include "no_gyro_fusion.h" +#include "quaternion_supervisor.h" +#include "mag_disturb.h" +#include "quat_accuracy_monitor.h" +#include "shake.h" diff --git a/6515/libsensors_iio/software/core/mpl/mag_disturb.h b/6515/libsensors_iio/software/core/mpl/mag_disturb.h new file mode 100644 index 0000000..33db531 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/mag_disturb.h @@ -0,0 +1,113 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+#ifndef MLDMP_MAGDISTURB_H__
+#define MLDMP_MAGDISTURB_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ // #define WIN_8
+ //#define RECORD_DATA_4_ANALYSIS
+
+ int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat,
+ const long *compass, const long *gravity);
+
+ void inv_track_world_yaw_angle_angle(int mode, float currdip);
+
+ inv_error_t inv_enable_magnetic_disturbance(void);
+ inv_error_t inv_disable_magnetic_disturbance(void);
+ int inv_get_magnetic_disturbance_state();
+
+ inv_error_t inv_set_magnetic_disturbance(int time_ms);
+ inv_error_t inv_init_magnetic_disturbance(void);
+
+ void inv_enable_magnetic_disturbance_logging(void);
+ void inv_disable_magnetic_disturbance_logging(void);
+
+ float Mag3ofNormalizedLong(const long *x);
+ float Mag2ofNormalizedLong(const long *x);
+ float Mag2ofNormalizedFloat(const float *x);
+
+ int inv_mag_disturb_get_detect_status_3D(void);
+ void inv_mag_disturb_set_detect_status_3D(int status);
+
+ int inv_mag_disturb_get_drop_heading_accuracy_status(void);
+ void inv_mag_disturb_set_drop_heading_accuracy_status(int status);
+
+ int inv_mag_disturb_get_detect_weak_status_3D(void);
+ void inv_mag_disturb_set_detect_weak_status_3D(int status);
+
+ int inv_mag_disturb_get_detect_world_yaw_angle_status(void);
+ void inv_mag_disturb_set_detect_world_yaw_angle_status(int status);
+
+ int inv_mag_disturb_get_detect_world_yaw_angle_confirm_status(void);
+ void inv_mag_disturb_set_detect_world_yaw_angle_confirm_status(int status);
+
+ float inv_mag_disturb_get_vector_radius_3D(void);
+ void inv_mag_disturb_set_vector_radius_3D(float radius);
+
+ void inv_mag_disturb_world_yaw_angle_init(void);
+ void inv_mag_disturb_world_yaw_angle_process(struct inv_sensor_cal_t *obj);
+ //enum mag_distub_state_e inv_mag_disturb_get_mar_world_yaw_angle_state(void);
+
+ char inv_mag_disturb_get_mar_world_yaw_angle_detection_status(void);
+
+ float inv_mag_disturb_world_yaw_angle_distortion_from_gyro_bias(struct inv_sensor_cal_t *obj);
+ int inv_mag_disturb_get_dip_compassNgravity(struct inv_sensor_cal_t *data);
+
+ float inv_mag_disturb_9x_quat_confidence_interval(struct inv_sensor_cal_t *obj);
+ float inv_mag_disturb_geo_mag_confidence_interval(struct inv_sensor_cal_t *obj);
+
+ float inv_mag_disturb_world_yaw_angle_distortion_from_accel_compass_bias(float accel_bias_error, float compass_bias_error);
+ float inv_mag_disturb_world_yaw_angle_distortion_from_accel_compass_only(float accel_bias_error, float compass_bias_error);
+
+ void inv_mag_disturb_all_confidence_interval_init(void);
+ void inv_mag_disturb_recover_compass_accuracy_to_3(void);
+
+#ifdef RECORD_DATA_4_ANALYSIS
+ void debug_file_init(void);
+ void debug_file_record(struct inv_sensor_cal_t *obj);
+ void debug_file_exit(void);
+#endif
+
+ /************************/
+ /* external API */
+ /************************/
+ float inv_mag_disturb_get_magnitude_threshold(void);
+ void inv_mag_disturb_set_magnitude_threshold(float radius);
+
+ float inv_mag_disturb_get_magnitude_recover_normal_threshold(void);
+ void inv_mag_disturb_set_magnitude_recover_normal_threshold(float radius);
+
+ /* These API get/set the yaw angle based magnetic anomaly detection*/
+ float inv_mag_disturb_get_yaw_angle_weak_threshold(void);
+ void inv_mag_disturb_set_yaw_angle_weak_threshold(float angle);
+
+ /* These API get/set the yaw angle based strong magnetic anomaly detection*/
+ /* The strong magnetic anomaly detection will drop heading accuracy if gyro is not trustable */
+ float inv_mag_disturb_get_yaw_angle_strong_threshold(void);
+ void inv_mag_disturb_set_yaw_angle_strong_threshold(float angle);
+
+ float inv_mag_disturb_get_time_threshold_detect(void);
+ void inv_mag_disturb_set_time_threshold_detect(float time_seconds);
+
+ float inv_mag_disturb_get_local_field_radius(void);
+ void inv_mag_disturb_set_local_field_radius(float radius);
+
+ float inv_mag_disturb_get_local_field_dip(void);
+ void inv_mag_disturb_set_local_field_dip(float dip);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_MAGDISTURB_H__
diff --git a/6515/libsensors_iio/software/core/mpl/motion_no_motion.h b/6515/libsensors_iio/software/core/mpl/motion_no_motion.h new file mode 100644 index 0000000..a43392c --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/motion_no_motion.h @@ -0,0 +1,31 @@ +/* + $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" +#include "invensense.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); +inv_time_t motion_nomot_get_gyro_bias_update_time(void); +void motion_nomot_set_gyro_bias_update_time(struct inv_sensor_cal_t *sensor_cal); + +#ifdef __cplusplus +} +#endif + +#endif // INV_MOTION_NO_MOTION_H__ diff --git a/6515/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/6515/libsensors_iio/software/core/mpl/no_gyro_fusion.h new file mode 100644 index 0000000..b1c23e0 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/no_gyro_fusion.h @@ -0,0 +1,35 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_NOGYROFUSION_H__ +#define MLDMP_NOGYROFUSION_H__ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_no_gyro_fusion(void); + inv_error_t inv_disable_no_gyro_fusion(void); + inv_error_t inv_start_no_gyro_fusion(void); + inv_error_t inv_start_no_gyro_fusion(void); + inv_error_t inv_init_no_gyro_fusion(void); + int inv_verify_no_gyro_fusion_data(float *data); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_NOGYROFUSION_H__ diff --git a/6515/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/6515/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h new file mode 100644 index 0000000..5ee0573 --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h @@ -0,0 +1,71 @@ +/* + 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__ diff --git a/6515/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/6515/libsensors_iio/software/core/mpl/quaternion_supervisor.h new file mode 100644 index 0000000..a2d445b --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/quaternion_supervisor.h @@ -0,0 +1,30 @@ +/* + $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 + +#define ACCELERATION_SQUARE_1P5G 9663676416LL +#define ACCELERATION_SQUARE_1P2G 6184752906LL +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); +int inv_verify_6x_fusion_data(float *data); + +#ifdef __cplusplus +} +#endif + +#endif // INV_QUATERNION_SUPERVISOR_H__ diff --git a/6515/libsensors_iio/software/core/mpl/shake.h b/6515/libsensors_iio/software/core/mpl/shake.h new file mode 100644 index 0000000..8775a4c --- /dev/null +++ b/6515/libsensors_iio/software/core/mpl/shake.h @@ -0,0 +1,94 @@ +/* + $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/6515/libsensors_iio/software/simple_apps/common/console_helper.c b/6515/libsensors_iio/software/simple_apps/common/console_helper.c new file mode 100644 index 0000000..2888627 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/common/console_helper.c @@ -0,0 +1,54 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +#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> + +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 +} diff --git a/6515/libsensors_iio/software/simple_apps/common/console_helper.h b/6515/libsensors_iio/software/simple_apps/common/console_helper.h new file mode 100644 index 0000000..5251d1c --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/common/console_helper.h @@ -0,0 +1,33 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +#ifndef _CONSOLE_HELPER_H_ +#define _CONSOLE_HELPER_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +//#include "mltypes.h" + +/* + Prototypes +*/ + +char ConsoleGetChar(void); +int ConsoleKbhit(void); + +#ifdef __cplusplus +} +#endif + +#endif // _CONSOLE_HELPER_H_ diff --git a/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.c new file mode 100644 index 0000000..25b0df6 --- /dev/null +++ b/6515/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/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/6515/libsensors_iio/software/simple_apps/common/mlerrorcode.h new file mode 100644 index 0000000..9a35792 --- /dev/null +++ b/6515/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/6515/libsensors_iio/software/simple_apps/common/testsupport.h b/6515/libsensors_iio/software/simple_apps/common/testsupport.h new file mode 100644 index 0000000..0cab781 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/common/testsupport.h @@ -0,0 +1,57 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: testsupport.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + ******************************************************************************/ + +#ifndef _TESTSUPPORT_H_ +#define _TESTSUPPORT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/*--------------------------- + Includes +---------------------------*/ + +#include "mltypes.h" +#include "mlerrorcode.h" + +#include "mlsl.h" +#include "log.h" + +/*--------------------------- + Defines +---------------------------*/ + +/*--------------------------- + p-Types +---------------------------*/ +#ifdef TESTING_SUPPORT + void SetHandle (void *sl_handle); + void CommandPrompt (void *sl_handle); + void RegisterMap (void *sl_handle); + void DataLogger (const unsigned long flag); + void DataLoggerSelector (const unsigned long flag); + void DataLoggerCb (void); + unsigned short KeyboardHandler (unsigned char key); + char* CompassStateName (char* out, int state); +#else +#define DataLoggerSelector(x) // +#define DataLogger(x) // +#define DataLoggerCb NULL +#endif + +#ifdef __cplusplus +} +#endif + +#endif // _TESTSUPPORT_H_ + diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/inv_devnode_parser-shared b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/inv_devnode_parser-shared Binary files differnew file mode 100755 index 0000000..092d7c4 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/inv_devnode_parser-shared diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/shared.mk new file mode 100644 index 0000000..38d1fb4 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/android/shared.mk @@ -0,0 +1,92 @@ +EXEC = inv_devnode_parser$(SHARED_APP_SUFFIX) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +APP_DIR = $(CURDIR)/../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MPL_DIR) +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += $(ANDROID_LINK_EXECUTABLE) + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") + $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/filelist.mk new file mode 100644 index 0000000..fdecbc8 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/build/filelist.mk @@ -0,0 +1,11 @@ +#### filelist.mk for inv_devnode_parser #### + +# headers +HEADERS += $(APP_DIR)/iio_utils.h + +# sources +SOURCES := $(APP_DIR)/read_device_node.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/iio_utils.h b/6515/libsensors_iio/software/simple_apps/devnode_parser/iio_utils.h new file mode 100644 index 0000000..c3d4955 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/iio_utils.h @@ -0,0 +1,650 @@ +/* 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/"; + +extern int verbose; + +/** + * 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 = 0; + FILE *sysfsfp; + int test; + char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + + if (verbose) + printf("VERB: echo %d > %s\n", val, temp); + 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); + fclose(sysfsfp); + if (verbose) + printf("VERB: cat %s = %d\n", temp, test); + if (test != val) { + printf("Possible failure in int write %d to %s\n", + val, temp); + 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); + fclose(sysfsfp); + + 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_se(const char *device_dir, struct iio_channel_info **ci_array, + int *counter, char *sensor, int en) +{ + DIR *dp; + int ret; + const struct dirent *ent; + char *scan_el_dir; + char pattern[50] = "in_"; + + *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; + } + strcat(pattern, sensor); + while (ent = readdir(dp), ent != NULL) { + if (strncmp(ent->d_name, pattern, strlen(pattern)) == 0 && + strncmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), + "_en", strlen("_en")) == 0) { + write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, en); + } + } + return 0; + +error_ret: +error_free_name: + return -1; +} + +int enable_accel_se(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, + int en) +{ + return enable_se(device_dir, ci_array, counter, "accel", en); +} + +int enable_anglvel_se(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, + int en) +{ + return enable_se(device_dir, ci_array, counter, "anglvel", en); +} + +int enable_quaternion_se(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, + int en) +{ + return enable_se(device_dir, ci_array, counter, "quaternion", en); +} + diff --git a/6515/libsensors_iio/software/simple_apps/devnode_parser/read_device_node.c b/6515/libsensors_iio/software/simple_apps/devnode_parser/read_device_node.c new file mode 100644 index 0000000..7b73d4a --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/devnode_parser/read_device_node.c @@ -0,0 +1,284 @@ +/* + * Copyright (c) Invensense Inc. 2012 + * + * 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. + */ + +#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 <termios.h> + +#include "iio_utils.h" +#include "ml_sysfs_helper.h" +#include "mlos.h" + +#define POLL_TIME (2000) // 2sec + +// settings +int verbose = false; + +// paths +char *dev_dir_name; + +/************************************************** + This _kbhit() function is courtesy of the web +***************************************************/ +int _kbhit(void) +{ + 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; +} + +void get_sensor_data(char *d, short *sensor) +{ + int i; + for (i = 0; i < 3; i++) + sensor[i] = *(short *)(d + 2 + i * 2); +} + +static int read_data(char *buffer_access) +{ +#define PRESSURE_HDR 0x8000 +#define ACCEL_HDR 0x4000 +#define GYRO_HDR 0x2000 +#define COMPASS_HDR 0x1000 +#define LPQUAT_HDR 0x0800 +#define SIXQUAT_HDR 0x0400 +#define PEDQUAT_HDR 0x0200 +#define STEP_DETECTOR_HDR 0x0100 + + static int left_over_size = 0; + char data[1048], *dptr, tmp[24]; + short sensor[3]; + int q[3]; + int ret, i, ind, fp; + int buf_size, read_size; + unsigned short hdr; + bool done_flag; + + 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_read_data; + } + ind = 0; + + { + struct pollfd pfd = { + .fd = fp, + .events = POLLIN, + }; + poll(&pfd, 1, -1); + + if (left_over_size > 0) + memcpy(data, tmp, left_over_size); + dptr = data + left_over_size; + + read_size = read(fp, dptr, 1024); + if (read_size <= 0) { + printf("Wrong size=%d\n", read_size); + ret = -EINVAL; + goto error_read_data; + } + + ind = read_size + left_over_size; + dptr = data; + buf_size = ind - (dptr - data); + done_flag = false; + while ((buf_size > 0) && (!done_flag)) { + hdr = *((short *)(dptr)); + if (hdr & 1) + printf("STEP\n"); + + switch (hdr & (~1)) { + case PRESSURE_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("PRESS, %d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case ACCEL_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("ACCEL, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case GYRO_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("GYRO, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case COMPASS_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("COMPASS, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case PEDQUAT_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("LOW_RES_QUAT, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case LPQUAT_HDR: + if (buf_size >= 24) { + q[0] = *(int *)(dptr + 4); + dptr += 8; + q[1] = *(int *)(dptr); + q[2] = *(int *)(dptr + 4); + dptr += 8; + printf("LPQ_3AXES, %d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); + } else + done_flag = true; + break; + case SIXQUAT_HDR: + if (buf_size >= 24) { + q[0] = *(int *)(dptr + 4); + dptr += 8; + q[1] = *(int *)(dptr); + q[2] = *(int *)(dptr + 4); + dptr += 8; + printf("LPQ_6AXES, %d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); + } else + done_flag = true; + break; + case STEP_DETECTOR_HDR: + if (buf_size >= 16) { + printf("STEP_DETECTOR, "); + dptr += 8; + printf("%lld\n", *(long long *)dptr); + } else + done_flag = true; + + break; + default: + printf("unknown, \n"); + for (i = 0; i < 8; i++) + printf("%02x, ", dptr[i]); + printf("\n"); + break; + } + if (!done_flag) + dptr += 8; + buf_size = ind - (dptr - data); + } + if (ind - (dptr - data) > 0) + memcpy(tmp, dptr, ind - (dptr - data)); + left_over_size = ind - (dptr - data); + } + close(fp); + +error_read_data: + return ret; +} + +/* + Main +*/ + +int main(int argc, char **argv) +{ + unsigned long num_loops = -1; + int ret, c, i; + + int dev_num; + char *buffer_access; + char chip_name[10]; + char *dummy; + char device_name[10]; + char sysfs[100]; + + // all output to stdout must be delivered immediately, no buffering + setvbuf(stdout, NULL, _IONBF, 0); + + /* parse the command line parameters + TODO description + */ + while ((c = getopt(argc, argv, "c:vh")) != -1) { + switch (c) { + case 'c': + num_loops = strtoul(optarg, &dummy, 10); + break; + case 'v': + verbose = true; + break; + case 'h': + //print_help(); + // TODO write print_help helper function + break; + case '?': + return -1; + } + } + + // get info about the device and driver + inv_get_sysfs_path(sysfs); + if (inv_get_chip_name(chip_name) != INV_SUCCESS) { + printf("get chip name fail\n"); + exit(0); + } + printf("INFO: chip_name=%s\n", chip_name); + + for (i = 0; i < strlen(chip_name); i++) + device_name[i] = tolower(chip_name[i]); + device_name[strlen(chip_name)] = '\0'; + printf("INFO: 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("INFO: iio device number=%d\n", dev_num); + + /* attempt to open non blocking the access dev */ + ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + while (num_loops == -1 || num_loops--) + read_data(buffer_access); + free(buffer_access); + +error_ret: + return ret; +} diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared Binary files differnew file mode 100755 index 0000000..be620fe --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/inv_gesture_test-shared diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk new file mode 100644 index 0000000..8591982 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/gesture_test/build/android/shared.mk @@ -0,0 +1,96 @@ +EXEC = inv_gesture_test$(SHARED_APP_SUFFIX) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +APP_DIR = $(CURDIR)/../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +MPL_DIR = $(INV_ROOT)/software/core/mpl + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MPL_DIR) +CFLAGS += -I$(COMMON_DIR) +CFLAGS += -I$(HAL_DIR)/include +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += $(ANDROID_LINK_EXECUTABLE) + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) +INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") + $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk new file mode 100644 index 0000000..75d93cf --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/gesture_test/build/filelist.mk @@ -0,0 +1,11 @@ +#### filelist.mk for inv_gesture_test #### + +# headers +#HEADERS += + +# sources +SOURCES := $(APP_DIR)/inv_gesture_test.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) diff --git a/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c b/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c new file mode 100644 index 0000000..ac0803f --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/gesture_test/inv_gesture_test.c @@ -0,0 +1,720 @@ +/** + * 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" +#include "mlos.h" + +//#define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */ + +#define SUPPORT_SCREEN_ORIENTATION +//#define SUPPORT_TAP +//#define SUPPORT_ORIENTATION +#define SUPPORT_PEDOMETER +#define SUPPORT_SMD + +#define MAX_SYSFS_NAME_LEN (100) +#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) +#define IIO_SYSFS_PATH "/sys/bus/iio/devices/iio:device0" +#define IIO_HUB_NAME "inv_hub" + +#define POLL_TIME (2000) // 2sec + +struct sysfs_attrbs { + char *name; + char *enable; + char *power_state; + char *dmp_on; + char *dmp_int_on; + char *dmp_firmware; + char *firmware_loaded; +#ifdef SUPPORT_SCREEN_ORIENTATION + char *event_display_orientation; + char *display_orientation_on; +#endif +#ifdef SUPPORT_ORIENTATION + char *event_orientation; + char *orientation_on; +#endif +#ifdef SUPPORT_TAP + char *event_tap; + char *tap_min_count; + char *tap_on; + char *tap_threshold; + char *tap_time; +#endif +#ifdef SUPPORT_PEDOMETER + char *pedometer_on; + char *pedometer_steps; + char *pedometer_time; +#endif +#ifdef SUPPORT_SMD + char *event_smd; + char *smd_enable; + char *smd_threshold; + char *smd_delay_threshold; + char *smd_delay_threshold2; +#endif +} mpu; + +enum { +#ifdef SUPPORT_TAP + FEAT_TAP, +#endif +#ifdef SUPPORT_SCREEN_ORIENTATION + FEAT_SCREEN_ORIENTATION, +#endif +#ifdef SUPPORT_ORIENTATION + FEAT_ORIENTATION, +#endif +#ifdef SUPPORT_PEDOMETER + FEAT_PEDOMETER, +#endif +#ifdef SUPPORT_SMD + FEAT_SMD, +#endif + + NUM_DMP_FEATS +}; + +char *sysfs_names_ptr; +#ifdef SUPPORT_PEDOMETER +unsigned long last_pedometer_poll = 0L; +unsigned long pedometer_poll_timeout = 500L; // .5 second +#endif +struct pollfd pfd[NUM_DMP_FEATS]; +bool android_hub = false; // flag to indicate true=Hub, false=non-hub + +/******************************************************************************* + * 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 { + printf("ERR open file to read: %s\n", filename); + res= -1; + } + return res; +} + +int write_sysfs_int(char *filename, int data) +{ + int res=0; + FILE *fp; + +#ifdef DEBUG_PRINT + printf("writing '%s' with '%d'\n", filename, data); +#endif + + fp = fopen(filename, "w"); + if (fp != NULL) { + fprintf(fp, "%d\n", data); + fclose(fp); + } else { + printf("ERR open file to write: %s\n", filename); + res = -1; + } + return res; +} + +/************************************************** + This _kbhit() function is courtesy of the web +***************************************************/ +int _kbhit(void) +{ + 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 { + printf("couldn't alloc mem for sysfs paths\n"); + return -1; + } + + // get proper (in absolute/relative) IIO path & build MPU's sysfs paths + inv_get_sysfs_path(sysfs_path); + + sprintf(mpu.name, "%s%s", sysfs_path, "/name"); + 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.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware"); + sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded"); + +#ifdef SUPPORT_SCREEN_ORIENTATION + sprintf(mpu.event_display_orientation, "%s%s", + sysfs_path, "/event_display_orientation"); + sprintf(mpu.display_orientation_on, "%s%s", + sysfs_path, "/display_orientation_on"); +#endif +#ifdef SUPPORT_ORIENTATION + sprintf(mpu.event_orientation, "%s%s", sysfs_path, "/event_orientation"); + sprintf(mpu.orientation_on, "%s%s", sysfs_path, "/orientation_on"); +#endif +#ifdef SUPPORT_TAP + sprintf(mpu.event_tap, "%s%s", sysfs_path, "/event_tap"); + 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"); +#endif +#ifdef SUPPORT_PEDOMETER + sprintf(mpu.pedometer_on, "%s%s", sysfs_path, "/dmp_on"); + sprintf(mpu.pedometer_steps, "%s%s", sysfs_path, "/pedometer_steps"); + sprintf(mpu.pedometer_time, "%s%s", sysfs_path, "/pedometer_time"); +#endif +#ifdef SUPPORT_SMD + sprintf(mpu.event_smd, "%s%s", sysfs_path, "/event_smd"); + sprintf(mpu.smd_enable, "%s%s", sysfs_path, "/smd_enable"); + sprintf(mpu.smd_threshold, "%s%s", sysfs_path, "/smd_threshold"); + sprintf(mpu.smd_delay_threshold, "%s%s", + sysfs_path, "/smd_delay_threshold"); + sprintf(mpu.smd_delay_threshold2, "%s%s", + sysfs_path, "/smd_delay_threshold2"); +#endif + +#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 dmp_fw_loaded(void) +{ + int fw_loaded; + if (read_sysfs_int(mpu.firmware_loaded, &fw_loaded) < 0) + fw_loaded= 0; + return fw_loaded; +} + +int is_android_hub(void) +{ + char dev_name[8]; + FILE *fp; + + fp= fopen(mpu.name, "r"); + fgets(dev_name, 8, fp); + fclose(fp); + + if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME))) { + android_hub = true; + }else { + android_hub = false; + } + + return 0; +} + +/* + Enablers for the gestures +*/ + +int master_enable(int en) +{ + if (write_sysfs_int(mpu.enable, en) < 0) { + printf("GT:ERR-can't write 'buffer/enable'"); + return -1; + } + return 0; +} + +#ifdef SUPPORT_TAP +int enable_tap(int en) +{ + if (write_sysfs_int(mpu.tap_on, en) < 0) { + printf("GT:ERR-can't write 'tap_on'\n"); + return -1; + } + + return 0; +} +#endif + +/* Unnecessary: pedometer_on == dmp_on, which is always on +#ifdef SUPPORT_PEDOMETER +int enable_pedometer(int en) +{ + if (write_sysfs_int(mpu.pedometer_on, en) < 0) { + printf("GT:ERR-can't write 'pedometer_on'\n"); + return -1; + } + + return 0; +} +#endif +*/ + +#ifdef SUPPORT_SCREEN_ORIENTATION +int enable_display_orientation(int en) +{ + if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { + printf("GT:ERR-can't write 'display_orientation_on'\n"); + return -1; + } + + return 0; +} +#endif + +#ifdef SUPPORT_ORIENTATION +int enable_orientation(int en) +{ + if (write_sysfs_int(mpu.orientation_on, en) < 0) { + printf("GT:ERR-can't write 'orientation_on'\n"); + return -1; + } + + return 0; +} +#endif + +#ifdef SUPPORT_SMD +int enable_smd(int en) +{ + if (write_sysfs_int(mpu.smd_enable, en) < 0) { + printf("GT:ERR-can't write 'smd_enable'\n"); + return -1; + } + return 0; +} +#endif + +/* + Handlers for the gestures +*/ +#ifdef SUPPORT_TAP +int tap_handler(void) +{ + 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, "); + break; + case 2: + printf("Tap Axis->X Neg, "); + break; + case 3: + printf("Tap Axis->Y Pos, "); + break; + case 4: + printf("Tap Axis->Y Neg, "); + break; + case 5: + printf("Tap Axis->Z Pos, "); + break; + case 6: + printf("Tap Axis->Z Neg, "); + break; + default: + printf("Tap Axis->Unknown, "); + break; + } + printf("#%d\n", tap_num); + + return 0; +} +#endif + +#ifdef SUPPORT_PEDOMETER +int pedometer_handler(void) +{ + FILE *fp; + static int last_pedometer_steps = -1; + static long last_pedometer_time = -1; + int pedometer_steps; + long pedometer_time; + +#ifdef DEBUG_PRINT + printf("GT:Pedometer Handler\n"); +#endif + + fp = fopen(mpu.pedometer_steps, "rt"); + fscanf(fp, "%d\n", &pedometer_steps); + fclose(fp); + + fp = fopen(mpu.pedometer_time, "rt"); + fscanf(fp, "%ld\n", &pedometer_time); + fclose(fp); + + if (last_pedometer_steps == -1 && last_pedometer_time == -1) { + printf("Pedometer Steps: %d Time: %ld ", + pedometer_steps, pedometer_time); + if (pedometer_steps > 10 + || pedometer_time > (pedometer_poll_timeout * 2)) + printf("(resumed)\n"); + else + printf("\n"); + } else if (last_pedometer_steps != pedometer_steps + || last_pedometer_time != pedometer_time) { + printf("Pedometer Steps: %d Time: %ld\n", + pedometer_steps, pedometer_time); + } + + last_pedometer_steps = pedometer_steps; + last_pedometer_time = pedometer_time; + + return 0; +} +#endif + +#ifdef SUPPORT_SCREEN_ORIENTATION +int display_orientation_handler(void) +{ + FILE *fp; + int orient; + +#ifdef DEBUG_PRINT + printf("GT:Screen Orient Handler\n"); +#endif + + fp = fopen(mpu.event_display_orientation, "rt"); + if (!fp) { + printf("GT:Cannot open '%s'\n", mpu.event_display_orientation); + return -1; + } + fscanf(fp, "%d\n", &orient); + fclose(fp); + + printf("Screen Orient-> %d\n", orient); + + return 0; +} +#endif + +#ifdef SUPPORT_ORIENTATION +int host_orientation_handler(void) +{ + 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; +} +#endif + +#ifdef SUPPORT_SMD +int smd_handler(void) +{ + FILE *fp; + int smd; + + fp = fopen(mpu.event_smd, "rt"); + fscanf(fp, "%d\n", &smd); + fclose(fp); + +#ifdef DEBUG_PRINT + printf("GT:SMD Handler\n"); +#endif + printf("SMD (%d)\n", smd); + + /* wait for the acceleration low pass filtered tail to die off - + this is to prevent that the tail end of a 2nd event of above threhsold + motion be considered as also the 1st event for the next SM detection */ + inv_sleep(1000); + + /* re-enable to continue the detection */ + master_enable(0); + enable_smd(1); + master_enable(1); + + return 0; +} +#endif + +int enable_dmp_features(int en) +{ + int res= -1; + + if (android_hub || dmp_fw_loaded()) { + /* 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 */ + + master_enable(0); +#ifdef SUPPORT_TAP + enable_tap(en); +#endif +#ifdef SUPPORT_SCREEN_ORIENTATION + enable_display_orientation(en); +#endif +#ifdef SUPPORT_ORIENTATION + if (android_hub == false) { + // Android Hub does not support 'regular' orientation feature + enable_orientation(en); + } +#endif +#ifdef SUPPORT_SMD + enable_smd(en); +#endif + master_enable(1); + res = 0; + + } else { + printf("GT:ERR-No DMP firmware\n"); + res= -1; + } + + return res; +} + +int init_fds(void) +{ + int i; + + for (i = 0; i < NUM_DMP_FEATS; i++) { + switch(i) { +#ifdef SUPPORT_TAP + case FEAT_TAP: + pfd[i].fd = open(mpu.event_tap, O_RDONLY | O_NONBLOCK); + break; +#endif +#ifdef SUPPORT_SCREEN_ORIENTATION + case FEAT_SCREEN_ORIENTATION: + pfd[i].fd = open(mpu.event_display_orientation, + O_RDONLY | O_NONBLOCK); + break; +#endif +#ifdef SUPPORT_ORIENTATION + case FEAT_ORIENTATION: + pfd[i].fd = open(mpu.event_orientation, O_RDONLY | O_NONBLOCK); + break; +#endif +#ifdef SUPPORT_SMD + case FEAT_SMD: + pfd[i].fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK); + break; +#endif + default: + pfd[i].fd = -1; + } + + pfd[i].events = POLLPRI|POLLERR, + pfd[i].revents = 0; + } + + return 0; +} + +void parse_events(struct pollfd pfd[], int num_fds) +{ + int i; + + for (i = 0; i < num_fds; i++) { + if(pfd[i].revents != 0) { + switch(i) { +#ifdef SUPPORT_TAP + case FEAT_TAP: + tap_handler(); + break; +#endif +#ifdef SUPPORT_SCREEN_ORIENTATION + case FEAT_SCREEN_ORIENTATION: + display_orientation_handler(); + break; +#endif +#ifdef SUPPORT_ORIENTATION + case FEAT_ORIENTATION: + host_orientation_handler(); + break; +#endif +#ifdef SUPPORT_SMD + case FEAT_SMD: + smd_handler(); + break; +#endif + default: + printf("GT:ERR-unhandled/unrecognized gesture event"); + break; + } + pfd[i].revents = 0; // no need: reset anyway + } + } + +#ifdef SUPPORT_PEDOMETER + { + unsigned long now; + // pedometer is not event based, therefore we poll using a timer every + // pedometer_poll_timeout milliseconds + if ((now = inv_get_tick_count()) - last_pedometer_poll + > pedometer_poll_timeout) { + pedometer_handler(); + last_pedometer_poll = now; + } + } +#endif +} + +int close_fds(void) +{ + int i; + for (i = 0; i < NUM_DMP_FEATS; i++) { + if (!pfd[i].fd) + close(pfd[i].fd); + } + return 0; +} + +/******************************************************************************* + * M a i n + ******************************************************************************/ + +int main(int argc, char **argv) +{ + char data[4]; + int i, res= 0; + + printf("\n" + "****************************************************************\n" + "*** NOTE: ***\n" + "*** the HAL must be compiled with Low power quaternion ***\n" + "*** and/or DMP screen orientation support. ***\n" + "*** 'At least' one of the 4 Android virtual sensors ***\n" + "*** must be enabled. ***\n" + "*** ***\n" + "*** Please perform gestures to see the output. ***\n" + "*** Press any key to stop the program. ***\n" + "****************************************************************\n" + "\n"); + + res = inv_init_sysfs_attributes(); + if (res) { + printf("GT:ERR-Can't allocate mem\n"); + return -1; + } + + /* check if Android Hub */ + is_android_hub(); + + /* init Fds to poll for gesture data */ + init_fds(); + + /* on Gesture/DMP supported features */ + if (enable_dmp_features(1) < 0) { + printf("GT:ERR-Can't enable Gestures\n"); + return -1; + } + + do { + for (i = 0; i < NUM_DMP_FEATS; i++) + read(pfd[i].fd, data, 4); + poll(pfd, NUM_DMP_FEATS, POLL_TIME); + parse_events(pfd, NUM_DMP_FEATS); + } while (!_kbhit()); + + /* off Gesture/DMP supported features */ + if (enable_dmp_features(0) < 0) { + printf("GT:ERR-Can't disable Gestures\n"); + return -1; + } + + /* release resources */ + close_fds(); + if (sysfs_names_ptr) + free(sysfs_names_ptr); + + return res; +} + diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared Binary files differnew file mode 100755 index 0000000..b524d37 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk new file mode 100644 index 0000000..2dda9e0 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk @@ -0,0 +1,94 @@ +EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +APP_DIR = $(CURDIR)/../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +MPL_DIR = $(INV_ROOT)/software/core/mpl + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MPL_DIR) +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += $(ANDROID_LINK_EXECUTABLE) + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) +INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") + $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk new file mode 100644 index 0000000..8a3977a --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk @@ -0,0 +1,12 @@ +#### 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/6515/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/6515/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h new file mode 100644 index 0000000..c3d4955 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h @@ -0,0 +1,650 @@ +/* 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/"; + +extern int verbose; + +/** + * 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 = 0; + FILE *sysfsfp; + int test; + char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + + if (verbose) + printf("VERB: echo %d > %s\n", val, temp); + 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); + fclose(sysfsfp); + if (verbose) + printf("VERB: cat %s = %d\n", temp, test); + if (test != val) { + printf("Possible failure in int write %d to %s\n", + val, temp); + 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); + fclose(sysfsfp); + + 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_se(const char *device_dir, struct iio_channel_info **ci_array, + int *counter, char *sensor, int en) +{ + DIR *dp; + int ret; + const struct dirent *ent; + char *scan_el_dir; + char pattern[50] = "in_"; + + *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; + } + strcat(pattern, sensor); + while (ent = readdir(dp), ent != NULL) { + if (strncmp(ent->d_name, pattern, strlen(pattern)) == 0 && + strncmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), + "_en", strlen("_en")) == 0) { + write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, en); + } + } + return 0; + +error_ret: +error_free_name: + return -1; +} + +int enable_accel_se(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, + int en) +{ + return enable_se(device_dir, ci_array, counter, "accel", en); +} + +int enable_anglvel_se(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, + int en) +{ + return enable_se(device_dir, ci_array, counter, "anglvel", en); +} + +int enable_quaternion_se(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, + int en) +{ + return enable_se(device_dir, ci_array, counter, "quaternion", en); +} + diff --git a/6515/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/6515/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c new file mode 100644 index 0000000..7a17392 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c @@ -0,0 +1,942 @@ +/* + * Copyright (c) Invensense Inc. 2012 + * + * 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. + */ + +#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 <termios.h> + +#include "iio_utils.h" +#include "ml_load_dmp.h" +#include "ml_sysfs_helper.h" +#include "authenticate.h" +#include "mlos.h" + +#define DMP_CODE_SIZE (3060) +#define POLL_TIME (2000) // 2sec + +// settings +static int accel_only = false; +static int test_motion = false; +static int test_flick = false; +static int test_pedometer = false; +static int test_orientation = false; +int verbose = false; + +// paths +char *dev_dir_name, *buf_dir_name; + +// all the DMP features supported +enum { + FEAT_TAP = 0, + FEAT_ORIENTATION, + FEAT_DISPLAY_ORIENTATION, + FEAT_MOTION, + FEAT_FLICK, + + FEAT_NUM, +} features; + +typedef void (*handler_t) (int data); + +struct dmp_feat_t { + int enabled; + struct pollfd *pollfd; + char *sysfs_name; + handler_t phandler; +}; + +static struct dmp_feat_t dmp_feat[FEAT_NUM] = {{0}}; +static struct pollfd pollfds[FEAT_NUM]; +static int pollfds_used = 0; + +/************************************************** + This _kbhit() function is courtesy of the web +***************************************************/ +int _kbhit(void) +{ + 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; +} + +/** + * 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"); +} + +/* + Enablers for the gestures +*/ + +int 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; + ret = write_sysfs_int_and_verify("flick_upper", p, 3147790); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("flick_lower", p, -3147790); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("flick_counter", p, 50); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("flick_message_on", p, 0); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("flick_axis", p, 0); + if (ret < 0) + return ret; + + return 0; +} + +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); +} + +int setup_dmp(char *dev_path, int p_event) +{ + char dmp_path[100]; + int ret; + FILE *fd; + + printf("INFO: sysfs path=%s\n", dev_path); + + ret = write_sysfs_int_and_verify("power_state", dev_path, 1); + if (ret < 0) + return ret; + + ret = write_sysfs_int("in_accel_scale", dev_path, 0); + if (ret < 0) + return ret; + ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); + if (ret < 0) + return ret; + ret = write_sysfs_int("sampling_frequency", dev_path, 200); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("firmware_loaded", dev_path, 0); + if (ret < 0) + return ret; + + 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("INFO: firmware_loaded=%d\n", + read_sysfs_posint("firmware_loaded", dev_path)); + + // set accel offsets + //ret = write_sysfs_int_and_verify("in_accel_x_offset", + // dev_path, 0xabcd0000); + //if (ret < 0) + // return ret; + //ret = write_sysfs_int_and_verify("in_accel_y_offset", + // dev_path, 0xffff0000); + //if (ret < 0) + // return ret; + //ret = write_sysfs_int_and_verify("in_accel_z_offset", + // dev_path, 0xcdef0000); + //if (ret < 0) + // return ret; + + ret = write_sysfs_int_and_verify("dmp_on", dev_path, 1); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("dmp_int_on", dev_path, 1); + if (ret < 0) + return ret; + + /* select which event to enable and interrupt on/off here */ + if (test_flick) { + ret = enable_flick(dev_path, 1); + if (ret < 0) + return ret; + } + + /* + ret = write_sysfs_int_and_verify("tap_on", dev_path, 1); + if (ret < 0) + return ret; + */ + + /*ret = write_sysfs_int_and_verify("display_orientation_on", + dev_path, 1); + if (ret < 0) + return ret;*/ + if (test_orientation) { + ret = write_sysfs_int_and_verify("orientation_on", dev_path, 1); + if (ret < 0) + return ret; + } + /* ret = write_sysfs_int_and_verify("dmp_output_rate", dev_path, 25); + if (ret < 0) + return ret;*/ + ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_path, p_event); + if (ret < 0) + return ret; + + //verify_img(dmp_path); + return 0; +} + +/* + Handlers for the gestures +*/ + +void handle_flick(int flick) +{ + printf("flick=%x\n", flick); +} + +void handle_display_orientation(int orient) +{ + printf("display_orientation=%x\n", orient); +} + +void handle_motion(int motion) +{ + printf("motion=%x\n", motion); +} + +void handle_orientation(int orient) +{ + printf("orientation="); + if (orient & 0x01) + printf("+X, "); + if (orient & 0x02) + printf("-X, "); + if (orient & 0x04) + printf("+Y, "); + if (orient & 0x08) + printf("-Y, "); + if (orient & 0x10) + printf("+Z, "); + if (orient & 0x20) + printf("-Z, "); + if (orient & 0x40) + printf("flip"); + printf("\n"); +} + +void handle_tap(int tap) +{ + int tap_dir = tap / 8; + int tap_num = tap % 8 + 1; + + printf("tap="); + switch (tap_dir) { + case 1: + printf("+X, "); + break; + case 2: + printf("-X, "); + break; + case 3: + printf("+Y, "); + break; + case 4: + printf("-Y, "); + break; + case 5: + printf("+Z, "); + break; + case 6: + printf("-Z, "); + break; + default: + break; + } + printf("#%d\n", tap_num); +} + +int handle_pedometer(int *got_event) +{ + static int last_pedometer_steps = -1; + static long last_pedometer_time = -1; + static unsigned long last_pedometer_poll = 0L; + static unsigned long pedometer_poll_timeout = 500L; // .5 second + + unsigned long now; + int pedometer_steps; + long pedometer_time; + +#ifdef DEBUG_PRINT + printf("GT:Pedometer Handler\n"); +#endif + + if ((now = inv_get_tick_count()) - last_pedometer_poll + < pedometer_poll_timeout) { + return 0; + } + last_pedometer_poll = now; + + pedometer_steps = read_sysfs_posint("pedometer_steps", dev_dir_name); + pedometer_time = read_sysfs_posint("pedometer_time", dev_dir_name); + + if (last_pedometer_steps == -1 && last_pedometer_time == -1) { + if (!*got_event) + printf("\n"); + printf("p> pedometer=%d, %ld ", + pedometer_steps, pedometer_time); + if (pedometer_steps > 10 + || pedometer_time > (pedometer_poll_timeout * 2)) + printf("(resumed)\n"); + else + printf("\n"); + *got_event = true; + } else if (last_pedometer_steps != pedometer_steps + || last_pedometer_time != pedometer_time) { + if (!*got_event) + printf("\n"); + printf("p> pedometer=%d, %ld\n", + pedometer_steps, pedometer_time); + *got_event = true; + } + + last_pedometer_steps = pedometer_steps; + last_pedometer_time = pedometer_time; + + return 0; +} + +/* + Main processing functions +*/ + +void dump_dmp_event_struct(void) +{ +#define VARVAL(f, v) printf("\t%s : " f "\n", #v, v); + int i; + + printf("dmp_feat structure content:\n"); + for (i = 0; i < FEAT_NUM; i++) { + printf("%d - ", i); + VARVAL("%d", dmp_feat[i].enabled); + VARVAL("%s", dmp_feat[i].sysfs_name); + VARVAL("%p", dmp_feat[i].phandler); + VARVAL("%p", dmp_feat[i].pollfd); + if (dmp_feat[i].pollfd) { + VARVAL("%d", dmp_feat[i].pollfd->events); + VARVAL("%d", dmp_feat[i].pollfd->revents); + VARVAL("%d", dmp_feat[i].pollfd->fd); + } + } + printf("dmp_feat structure content:\n"); + for (i = 0; i < FEAT_NUM; i++) { + printf("%d - ", i); + VARVAL("%d", pollfds[i].fd); + VARVAL("%d", pollfds[i].events); + VARVAL("%d", pollfds[i].revents); + } + printf("end.\n"); +} + +void init_dmp_event_fds(void) +{ + int i, j = 0; + char file_name[100]; + + for (i = 0; i < FEAT_NUM; i++) { + if (!dmp_feat[i].enabled) + continue; + sprintf(file_name, "%s/%s", dev_dir_name, dmp_feat[i].sysfs_name); + pollfds[j].fd = open(file_name, O_RDONLY | O_NONBLOCK); + if (pollfds[j].fd < 0) { + printf("Err: cannot open requested event file '%s'\n", file_name); + } else { + printf("INFO: opened event node '%s'\n", file_name); + } + pollfds[j].events = POLLPRI | POLLERR; + pollfds[j].revents = 0; + + dmp_feat[i].pollfd = &pollfds[j]; + j++; + } +} + +void close_dmp_event_fds(void) +{ + int i; + for (i = 0; i < pollfds_used; i++) + close(pollfds[i].fd); +} + +void poll_dmp_event_fds(void) +{ + int i; + char d[4]; + static int got_event = 1; + + // read the pollable fds + for (i = 0; i < pollfds_used; i++) + read(pollfds[i].fd, d, 4); + + // poll + if (got_event) + printf("e> "); + got_event = false; + poll(pollfds, pollfds_used, POLL_TIME); + + for (i = 0; i < FEAT_NUM; i++) { + if (!dmp_feat[i].enabled) + continue; + + if (dmp_feat[i].pollfd->revents != 0) { + char file_name[200]; + int data; + + sprintf(file_name, "%s/%s", + dev_dir_name, dmp_feat[i].sysfs_name); + FILE *fp = fopen(file_name, "rt"); + if (!fp) { + printf("Err:cannot open requested event file '%s'\n", + dmp_feat[i].sysfs_name); + continue; + } + fscanf(fp, "%d\n", &data); + fclose(fp); + dmp_feat[i].pollfd->revents = 0; + + dmp_feat[i].phandler(data); + got_event = true; + } + } + + if (test_pedometer) { + /* pedometer is not event based, therefore we poll using a timer every + pedometer_poll_timeout milliseconds */ + handle_pedometer(&got_event); + } +} + +/* + Main +*/ + +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; + + 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; + + // all output to stdout must be delivered immediately, no buffering + setvbuf(stdout, NULL, _IONBF, 0); + + // get info about the device and driver + inv_get_sysfs_path(sysfs); + if (inv_get_chip_name(chip_name) != INV_SUCCESS) { + printf("get chip name fail\n"); + exit(0); + } + printf("INFO: chip_name=%s\n", chip_name); + + for (i = 0; i < strlen(chip_name); i++) + device_name[i] = tolower(chip_name[i]); + device_name[strlen(chip_name)] = '\0'; + printf("INFO: device name=%s\n", device_name); + + /* parse the command line parameters + -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:premavt:")) != -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 'm': + test_motion = true; + break; + case 'a': + accel_only = true; + break; + case 'v': + verbose = true; + break; + case '?': + return -1; + } + } + + pollfds_used = 0; + + // comment out/remove/if(0) the block corresponding to the feature + // that you want to disable + + if (0) { + struct dmp_feat_t f = { + true, + NULL, + "event_tap", + handle_tap + }; + dmp_feat[pollfds_used] = f; + pollfds_used++; + } + if (test_orientation) { + struct dmp_feat_t f = { + true, + NULL, + "event_orientation", + handle_orientation + }; + dmp_feat[pollfds_used] = f; + pollfds_used++; + } + /*if (1) { + struct dmp_feat_t f = { + true, + NULL, + "event_display_orientation", + handle_display_orientation + }; + dmp_feat[pollfds_used] = f; + pollfds_used++; + }*/ + if (test_motion) { + struct dmp_feat_t f = { + true, + NULL, + "event_accel_motion", + handle_motion + }; + dmp_feat[pollfds_used] = f; + pollfds_used++; + } + if (test_flick) { + struct dmp_feat_t f = { + true, + NULL, + "event_flick", + handle_flick + }; + dmp_feat[pollfds_used] = f; + pollfds_used++; + } + + // debug + printf("INFO\n"); + printf("INFO: Configured features:\n"); + for (i = 0; i < pollfds_used; i++) + printf("INFO: %d -> %s\n", i, dmp_feat[i].sysfs_name); + printf("INFO\n"); + + /* 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("INFO: iio device number=%d\n", dev_num); + asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num); + if (trigger_name == NULL) { + /* + * Build the trigger name. If it is device associated it's + * name is <device_name>_dev[n] where n matches the device + * number found above + */ + ret = asprintf(&trigger_name, "%s-dev%d", device_name, dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + } + + ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, 0); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("buffer/enable", dev_dir_name, 0); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); + + // + // motion interrupt in low power accel mode + // + if (test_motion) { + ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, 1); + if (ret < 0) + return ret; + // magnitude threshold - range [0, 1020] in 32 mg increments + ret = write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, + 3 * 32); + if (ret < 0) + return ret; + // duration in ms up to 2^16 + // ret = write_sysfs_int_and_verify("motion_lpa_dur", dev_dir_name, + // 200 * 1); + //if (ret < 0) + // return ret; + // motion_lpa_freq: 0 for 1.25, 1 for 5, 2 for 20, 3 for 40 Hz update rate + // of the low power accel mode. + // The higher the rate, the better responsiveness of the motion interrupt. + ret = write_sysfs_int("motion_lpa_freq", dev_dir_name, 2); + if (ret < 0) + return ret; + } else { + ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, 0); + if (ret < 0) + return ret; + } + + /* 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("INFO: iio trigger number=%d\n", trig_num); + + 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; + } + + /* 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; + + // gyro + if (accel_only) { + ret = enable_anglvel_se(dev_dir_name, &infoarray, &num_channels, 0); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 0); + if (ret < 0) + return ret; + } else { + ret = enable_anglvel_se(dev_dir_name, &infoarray, &num_channels, 1); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); + if (ret < 0) + return ret; + } + + // accel + ret = enable_accel_se(dev_dir_name, &infoarray, &num_channels, 1); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, 1); + if (ret < 0) + return ret; + + // quaternion + if (!nodmp) { + ret = enable_quaternion_se(dev_dir_name, &infoarray, &num_channels, 1); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, 1); + if (ret < 0) + return ret; + } else { + ret = enable_quaternion_se(dev_dir_name, &infoarray, &num_channels, 0); + if (ret < 0) + return ret; + ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); + if (ret < 0) + return ret; + } + + //sprintf(dmp_path, "%s/dmp_firmware", dev_dir_name); + //verify_img(dmp_path); + + 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_and_verify("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; + } +/*ADDED*/ + ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, 1); + if (ret < 0) + return ret; + if (p_event) { + + /* polling events from the DMP */ + init_dmp_event_fds(); + while(!_kbhit()) + poll_dmp_event_fds(); + close_dmp_event_fds(); + + } else { + + /* attempt to open non blocking the access dev */ + ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_data; + } + 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 num_loops 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 (!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_and_verify("enable", buf_dir_name, 0); + /* disable the dmp */ + if (p_event) + ret = write_sysfs_int_and_verify("dmp_on", dev_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/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c new file mode 100644 index 0000000..c4f09a6 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.c @@ -0,0 +1,379 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/* + Includes, Defines, and Macros +*/ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 /* turn to 0 to enable verbose logging */ + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-playback" + +#include "and_constructor.h" +#include "mlos.h" +#include "invensense.h" +#include "invensense_adv.h" + +/* + Typedef +*/ +struct inv_construct_t { + int product; /**< Gyro Product Number */ + int debug_mode; + int last_mode; + FILE *file; + int dmp_version; + int left_in_buffer; +#define FIFO_READ_SIZE 100 + unsigned char fifo_data[FIFO_READ_SIZE]; + int gyro_enable; + int accel_enable; + int compass_enable; + int quat_enable; +}; + +/* + Globals +*/ +static struct inv_construct_t inv_construct = {0}; +static void (*s_func_cb)(void); +static char playback_filename[101] = "/data/playback.bin"; +struct fifo_dmp_config fifo_dmp_cfg = {0}; + +/* + Functions +*/ +void inv_set_playback_filename(char *filename, int length) +{ + if (length > 100) { + MPL_LOGE("Error : file name and path too long, 100 characters limit\n"); + return; + } + strncpy(playback_filename, filename, length); +} + +inv_error_t inv_constructor_setup(void) +{ + unsigned short orient; + extern signed char g_gyro_orientation[9]; + extern signed char g_accel_orientation[9]; + extern signed char g_compass_orientation[9]; + float scale = 2.f; + long sens; + + // 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); + scale = 2.f; + sens = (long)(scale * (1L << 15)); + inv_set_accel_orientation_and_scale(orient, sens); + + // compass setup + orient = inv_orientation_matrix_to_scalar(g_compass_orientation); + // scale is the max value of the compass in micro Tesla. + scale = 5000.f; + sens = (long)(scale * (1L << 15)); + inv_set_compass_orientation_and_scale(orient, sens); + + return INV_SUCCESS; +} + +inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)) +{ + s_func_cb = func_cb; + return INV_SUCCESS; +} + +void int32_to_long(int32_t in[], long out[], int length) +{ + int ii; + for (ii = 0; ii < length; ii++) + out[ii] = (long)in[ii]; +} + +inv_error_t inv_playback(void) +{ + inv_rd_dbg_states type; + inv_time_t ts; + int32_t buffer[4]; + short gyro[3]; + size_t r = 1; + int32_t orientation; + int32_t sensitivity, sample_rate_us = 0; + + // Check to make sure we were request to playback + if (inv_construct.debug_mode != RD_PLAYBACK) { + MPL_LOGE("%s|%s|%d error: debug_mode != RD_PLAYBACK\n", + __FILE__, __func__, __LINE__); + return INV_ERROR; + } + + if (inv_construct.file == NULL) { + inv_construct.file = fopen(playback_filename, "rb"); + if (!inv_construct.file) { + MPL_LOGE("Error : cannot find or open playback file '%s'\n", + playback_filename); + return INV_ERROR_FILE_OPEN; + } + } + + while (1) { + r = fread(&type, sizeof(type), 1, inv_construct.file); + if (r == 0) { + MPL_LOGV("read 0 bytes, PLAYBACK file closed\n"); + inv_construct.debug_mode = RD_NO_DEBUG; + fclose(inv_construct.file); + break; + } + //MPL_LOGV("TYPE : %d, %d\n", type); + switch (type) { + case PLAYBACK_DBG_TYPE_GYRO: + r = fread(gyro, sizeof(gyro[0]), 3, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + inv_build_gyro(gyro, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO, %+d, %+d, %+d, %+lld\n", + gyro[0], gyro[1], gyro[2], ts); + break; + case PLAYBACK_DBG_TYPE_ACCEL: + { + long accel[3]; + r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + int32_to_long(buffer, accel, 3); + inv_build_accel(accel, 0, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL, %+d, %+d, %+d, %lld\n", + buffer[0], buffer[1], buffer[2], ts); + break; + } + case PLAYBACK_DBG_TYPE_COMPASS: + { + long compass[3]; + r = fread(buffer, sizeof(buffer[0]), 3, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + int32_to_long(buffer, compass, 3); + inv_build_compass(compass, 0, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS, %+d, %+d, %+d, %lld\n", + buffer[0], buffer[1], buffer[2], ts); + break; + } + case PLAYBACK_DBG_TYPE_TEMPERATURE: + r = fread(buffer, sizeof(buffer[0]), 1, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + inv_build_temp(buffer[0], ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_TEMPERATURE, %+d, %lld\n", + buffer[0], ts); + break; + case PLAYBACK_DBG_TYPE_QUAT: + { + long quat[4]; + r = fread(buffer, sizeof(buffer[0]), 4, inv_construct.file); + r = fread(&ts, sizeof(ts), 1, inv_construct.file); + int32_to_long(buffer, quat, 4); + inv_build_quat(quat, INV_BIAS_APPLIED, ts); + MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT, %+d, %+d, %+d, %+d, %lld\n", + buffer[0], buffer[1], buffer[2], buffer[3], ts); + break; + } + case PLAYBACK_DBG_TYPE_EXECUTE: + MPL_LOGV("PLAYBACK_DBG_TYPE_EXECUTE\n"); + inv_execute_on_data(); + if (s_func_cb) + s_func_cb(); + //done = 1; + break; + + case PLAYBACK_DBG_TYPE_G_ORIENT: + MPL_LOGV("PLAYBACK_DBG_TYPE_G_ORIENT\n"); + r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); + r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); + inv_set_gyro_orientation_and_scale(orientation, sensitivity); + break; + case PLAYBACK_DBG_TYPE_A_ORIENT: + MPL_LOGV("PLAYBACK_DBG_TYPE_A_ORIENT\n"); + r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); + r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); + inv_set_accel_orientation_and_scale(orientation, sensitivity); + break; + case PLAYBACK_DBG_TYPE_C_ORIENT: + MPL_LOGV("PLAYBACK_DBG_TYPE_C_ORIENT\n"); + r = fread(&orientation, sizeof(orientation), 1, inv_construct.file); + r = fread(&sensitivity, sizeof(sensitivity), 1, inv_construct.file); + inv_set_compass_orientation_and_scale(orientation, sensitivity); + break; + + case PLAYBACK_DBG_TYPE_G_SAMPLE_RATE: + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_gyro_sample_rate(sample_rate_us); + MPL_LOGV("PLAYBACK_DBG_TYPE_G_SAMPLE_RATE => %d\n", + sample_rate_us); + break; + case PLAYBACK_DBG_TYPE_A_SAMPLE_RATE: + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_accel_sample_rate(sample_rate_us); + MPL_LOGV("PLAYBACK_DBG_TYPE_A_SAMPLE_RATE => %d\n", + sample_rate_us); + break; + case PLAYBACK_DBG_TYPE_C_SAMPLE_RATE: + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_compass_sample_rate(sample_rate_us); + MPL_LOGV("PLAYBACK_DBG_TYPE_C_SAMPLE_RATE => %d\n", + sample_rate_us); + break; + + case PLAYBACK_DBG_TYPE_GYRO_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_GYRO_OFF\n"); + inv_gyro_was_turned_off(); + break; + case PLAYBACK_DBG_TYPE_ACCEL_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_ACCEL_OFF\n"); + inv_accel_was_turned_off(); + break; + case PLAYBACK_DBG_TYPE_COMPASS_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_COMPASS_OFF\n"); + inv_compass_was_turned_off(); + break; + case PLAYBACK_DBG_TYPE_QUAT_OFF: + MPL_LOGV("PLAYBACK_DBG_TYPE_QUAT_OFF\n"); + inv_quaternion_sensor_was_turned_off(); + break; + + case PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE: + MPL_LOGV("PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE\n"); + r = fread(&sample_rate_us, sizeof(sample_rate_us), + 1, inv_construct.file); + inv_set_quat_sample_rate(sample_rate_us); + break; + default: + //MPL_LOGV("PLAYBACK file closed\n"); + fclose(inv_construct.file); + MPL_LOGE("%s|%s|%d error: unrecognized log type '%d', " + "PLAYBACK file closed\n", + __FILE__, __func__, __LINE__, type); + return INV_ERROR; + } + } + msleep(1); + + inv_construct.debug_mode = RD_NO_DEBUG; + fclose(inv_construct.file); + + return INV_SUCCESS; +} + +/** Turns on/off playback and record modes +* @param mode Turn on recording mode with RD_RECORD and turn off recording mode with +* RD_NO_DBG. Turn on playback mode with RD_PLAYBACK. +*/ +void inv_set_debug_mode(rd_dbg_mode mode) +{ +#ifdef INV_PLAYBACK_DBG + inv_construct.debug_mode = mode; +#endif +} + +inv_error_t inv_constructor_start(void) +{ + inv_error_t result; + unsigned char divider; + //int gest_enabled = inv_get_gesture_enable(); + + // start the software + result = inv_start_mpl(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* + if (inv_construct.dmp_version == WIN8_DMP_VERSION) { + int fifo_divider; + divider = 4; // 4 means 200Hz DMP + fifo_divider = 3; + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_gyro_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_quat_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + + // Set Compass Sample Rate in MPL in micro seconds + inv_set_compass_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + + // Set Accel Sample Rate in MPL in micro seconds + inv_set_accel_sample_rate(1000L*(divider+1)*(fifo_divider+1)); + } else if (gest_enabled) { + int fifo_divider; + unsigned char mpl_divider; + + inv_send_interrupt_word(); + inv_send_sensor_data(INV_ALL & INV_GYRO_ACC_MASK); + inv_send_quaternion(); + + divider = fifo_dmp_cfg.sample_divider; + mpl_divider = fifo_dmp_cfg.mpl_divider; + + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_gyro_sample_rate(1000L*(mpl_divider+1)); + + // Set Gyro Sample Rate in MPL in micro seconds + inv_set_quat_sample_rate(1000L*(mpl_divider+1)); + + // Set Compass Sample Rate in MPL in micro seconds + inv_set_compass_sample_rate(1000L*(mpl_divider+1)); + + // Set Accel Sample Rate in MPL in micro seconds + inv_set_accel_sample_rate(1000L*(mpl_divider+1)); + } else + */ + { + divider = 9; + // set gyro sample sate in MPL in micro seconds + inv_set_gyro_sample_rate(1000L*(divider+1)); + // set compass sample rate in MPL in micro seconds + inv_set_compass_sample_rate(1000L*(divider+1)); + // set accel sample rate in MPL in micro seconds + inv_set_accel_sample_rate(1000L*(divider+1)); + } + + // setup the scale factors and orientations and other parameters + result = inv_constructor_setup(); + + return result; +} + +inv_error_t inv_constructor_default_enable() +{ + INV_ERROR_CHECK(inv_enable_quaternion()); + INV_ERROR_CHECK(inv_enable_fast_nomot()); + INV_ERROR_CHECK(inv_enable_heading_from_gyro()); + INV_ERROR_CHECK(inv_enable_compass_bias_w_gyro()); + INV_ERROR_CHECK(inv_enable_hal_outputs()); + INV_ERROR_CHECK(inv_enable_vector_compass_cal()); + INV_ERROR_CHECK(inv_enable_9x_sensor_fusion()); + INV_ERROR_CHECK(inv_enable_gyro_tc()); + INV_ERROR_CHECK(inv_enable_no_gyro_fusion()); + INV_ERROR_CHECK(inv_enable_in_use_auto_calibration()); + INV_ERROR_CHECK(inv_enable_magnetic_disturbance()); + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.h b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.h new file mode 100644 index 0000000..5905037 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/and_constructor.h @@ -0,0 +1,62 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef INV_CONSTRUCTOR_H__ +#define INV_CONSTRUCTOR_H__ + +#include "mltypes.h" +#include "data_builder.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define PRECISION 10000.f +#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) { \ + range.mantissa = (long)x; \ + range.fraction = (long)((float)(x-(long)x)*PRECISION); \ +} +#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) { \ + x = (float)(range.mantissa); \ + x += ((float)range.fraction/PRECISION); \ +} + +struct fifo_dmp_config { + unsigned char sample_divider; + unsigned char fifo_divider; + unsigned char mpl_divider; +}; + +inv_error_t inv_construct_and_push_data(); +inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)); +inv_error_t inv_constructor_setup(); +inv_error_t inv_constructor_start(); +inv_error_t inv_constructor_init(); +inv_error_t inv_constructor_default_enable(); +void inv_set_debug_mode(rd_dbg_mode mode); +inv_error_t inv_playback(); +void inv_set_playback_filename(char *filename, int length); +inv_error_t wait_for_and_process_interrupt(); + +inv_error_t inv_set_interrupt_word(unsigned long word); +inv_error_t inv_get_interrupt_word(unsigned long *data); +inv_error_t inv_set_gesture_enable(int word); +int inv_get_gesture_enable(void); +inv_error_t inv_set_fifo_rate(unsigned long fifo_rate); +inv_error_t inv_get_dmp_sample_divider(unsigned char *data); + +#ifdef __cplusplus +} +#endif + +#endif // INVENSENSE_INV_CONSTRUCTOR_H__ + diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/inv_playback-shared b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/inv_playback-shared Binary files differnew file mode 100755 index 0000000..9252ad1 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/inv_playback-shared diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/shared.mk new file mode 100644 index 0000000..dc33ee3 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/build/android/shared.mk @@ -0,0 +1,96 @@ +EXEC = inv_playback$(SHARED_APP_SUFFIX) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../../.. +APP_DIR = $(CURDIR)/../.. +COMMON_DIR = $(INV_ROOT)/software/simple_apps/common +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +MPL_DIR = $(INV_ROOT)/software/core/mpl + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MPL_DIR) +CFLAGS += -I$(COMMON_DIR) +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += $(ANDROID_LINK_EXECUTABLE) + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) +INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") + $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/playback/linux/build/filelist.mk new file mode 100644 index 0000000..1d04fea --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/build/filelist.mk @@ -0,0 +1,20 @@ +#### filelist.mk for mpu_iio #### + +# headers +HEADERS += $(APP_DIR)/iio_utils.h +HEADERS += $(APP_DIR)/and_constructor.h +HEADERS += $(APP_DIR)/datalogger_outputs.h +HEADERS += $(COMMON_DIR)/console_helper.h +HEADERS += $(COMMON_DIR)/mlerrorcode.h +HEADERS += $(COMMON_DIR)/testsupport.h + +# sources +SOURCES := $(APP_DIR)/main.c +SOURCES += $(APP_DIR)/and_constructor.c +SOURCES += $(APP_DIR)/datalogger_outputs.c +SOURCES += $(COMMON_DIR)/console_helper.c +SOURCES += $(COMMON_DIR)/mlerrorcode.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.c b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.c new file mode 100644 index 0000000..7c81cbb --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.c @@ -0,0 +1,384 @@ +/** + * @defgroup HAL_Outputs + * @brief Motion Library - HAL Outputs + * Sets up common outputs for HAL + * + * @{ + * @file datalogger_outputs.c + * @brief Windows 8 HAL outputs. + */ + +#include <string.h> + +#include "datalogger_outputs.h" +#include "ml_math_func.h" +#include "mlmath.h" +#include "start_manager.h" +#include "data_builder.h" +#include "results_holder.h" + +/* + Defines +*/ +#define ACCEL_CONVERSION (0.000149637603759766f) + +/* + Types +*/ +struct datalogger_output_s { + int quat_accuracy; + inv_time_t quat_timestamp; + long quat[4]; + struct inv_sensor_cal_t sc; +}; + +/* + Globals and Statics +*/ +static struct datalogger_output_s dl_out; + +/* + Functions +*/ + +/** + * Raw (uncompensated) angular velocity (LSB) in chip frame. + * @param[out] values raw angular velocity in LSB. + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_gyro_raw_short(short *values, inv_time_t *timestamp) +{ + struct inv_single_sensor_t *pg = &dl_out.sc.gyro; + + if (values) + memcpy(values, &pg->raw, sizeof(short) * 3); + if (timestamp) + *timestamp = pg->timestamp; +} + +/** + * Raw (uncompensated) angular velocity (degrees per second) in body frame. + * @param[out] values raw angular velocity in dps. + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_gyro_raw_body_float(float *values, + inv_time_t *timestamp) +{ + struct inv_single_sensor_t *pg = &dl_out.sc.gyro; + long raw[3]; + long raw_body[3]; + + raw[0] = (long) pg->raw[0] * (1L << 16); + raw[1] = (long) pg->raw[1] * (1L << 16); + raw[2] = (long) pg->raw[2] * (1L << 16); + inv_convert_to_body_with_scale(pg->orientation, pg->sensitivity, + raw, raw_body); + if (values) { + values[0] = inv_q16_to_float(raw_body[0]); + values[1] = inv_q16_to_float(raw_body[1]); + values[2] = inv_q16_to_float(raw_body[2]); + } + if (timestamp) + *timestamp = pg->timestamp; +} + +/** + * Angular velocity (degrees per second) in body frame. + * @param[out] values Angular velocity in dps. + * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_gyro_float(float *values, int8_t *accuracy, + inv_time_t *timestamp) +{ + long gyro[3]; + inv_get_gyro_set(gyro, accuracy, timestamp); + + values[0] = (float)gyro[0] / 65536.f; + values[1] = (float)gyro[1] / 65536.f; + values[2] = (float)gyro[2] / 65536.f; +} + +/** + * Raw (uncompensated) acceleration (LSB) in chip frame. + * @param[out] values raw acceleration in LSB. + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_accel_raw_short(short *values, inv_time_t *timestamp) +{ + struct inv_single_sensor_t *pa = &dl_out.sc.accel; + + if (values) + memcpy(values, &pa->raw, sizeof(short) * 3); + if (timestamp) + *timestamp = pa->timestamp; +} + +/** + * Acceleration (g's) in body frame. + * Microsoft defines gravity as positive acceleration pointing towards the + * Earth. + * @param[out] values Acceleration in g's. + * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_accel_float(float *values, int8_t *accuracy, + inv_time_t *timestamp) +{ + long accel[3]; + inv_get_accel_set(accel, accuracy, timestamp); + + values[0] = (float) -accel[0] / 65536.f; + values[1] = (float) -accel[1] / 65536.f; + values[2] = (float) -accel[2] / 65536.f; +} + +/** + * Raw (uncompensated) compass magnetic field (LSB) in chip frame. + * @param[out] values raw magnetic field in LSB. + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_compass_raw_short(short *values, inv_time_t *timestamp) +{ + struct inv_single_sensor_t *pc = &dl_out.sc.compass; + + if (values) + memcpy(values, &pc->raw, sizeof(short) * 3); + if (timestamp) + *timestamp = pc->timestamp; +} + +/** + * Magnetic heading/field strength in body frame. + * TODO: No difference between mag_north and true_north yet. + * @param[out] mag_north Heading relative to magnetic north in degrees. + * @param[out] true_north Heading relative to true north in degrees. + * @param[out] values Field strength in milligauss. + * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_compass_float(float *mag_north, float *true_north, + float *values, int8_t *accuracy, inv_time_t *timestamp) +{ + long compass[3]; + long q00, q12, q22, q03, t1, t2; + + /* 1 uT = 10 milligauss. */ +#define COMPASS_CONVERSION (10 / 65536.f) + inv_get_compass_set(compass, accuracy, timestamp); + if (values) { + values[0] = (float)compass[0]*COMPASS_CONVERSION; + values[1] = (float)compass[1]*COMPASS_CONVERSION; + values[2] = (float)compass[2]*COMPASS_CONVERSION; + } + + /* TODO: Stolen from euler angle computation. Calculate this only once per + * callback. + */ + q00 = inv_q29_mult(dl_out.quat[0], dl_out.quat[0]); + q12 = inv_q29_mult(dl_out.quat[1], dl_out.quat[2]); + q22 = inv_q29_mult(dl_out.quat[2], dl_out.quat[2]); + q03 = inv_q29_mult(dl_out.quat[0], dl_out.quat[3]); + t1 = q12 - q03; + t2 = q22 + q00 - (1L << 30); + if (mag_north) { + *mag_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; + if (*mag_north < 0) + *mag_north += 360; + } + if (true_north) { + if (!mag_north) { + *true_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; + if (*true_north < 0) + *true_north += 360; + } else { + *true_north = *mag_north; + } + } +} + +#if 0 +// put it back when we handle raw temperature +/** + * Raw temperature (LSB). + * @param[out] value raw temperature in LSB (1 element). + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_temp_raw_short(short *value, inv_time_t *timestamp) +{ + struct inv_single_sensor_t *pt = &dl_out.sc.temp; + if (value) { + /* no raw temperature, temperature is only handled calibrated + *value = pt->raw[0]; + */ + *value = pt->calibrated[0]; + } + if (timestamp) + *timestamp = pt->timestamp; +} +#endif + +/** + * Temperature (degree C). + * @param[out] values Temperature in degrees C. + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_temperature_float(float *value, inv_time_t *timestamp) +{ + struct inv_single_sensor_t *pt = &dl_out.sc.temp; + long ltemp; + if (timestamp) + *timestamp = pt->timestamp; + if (value) { + /* no raw temperature, temperature is only handled calibrated + ltemp = pt->raw[0]; + */ + ltemp = pt->calibrated[0]; + *value = (float) ltemp / (1L << 16); + } +} + +/** + * Quaternion in body frame. + * @e inv_get_sensor_type_quaternion_float outputs the data in the following + * order: X, Y, Z, W. + * TODO: Windows expects a discontinuity at 180 degree rotations. Will our + * convention be ok? + * @param[out] values Quaternion normalized to one. + * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). + * @param[out] timestamp Time when sensor was sampled. + */ +void inv_get_sensor_type_quat_float(float *values, int *accuracy, + inv_time_t *timestamp) +{ + values[0] = dl_out.quat[0] / 1073741824.f; + values[1] = dl_out.quat[1] / 1073741824.f; + values[2] = dl_out.quat[2] / 1073741824.f; + values[3] = dl_out.quat[3] / 1073741824.f; + accuracy[0] = dl_out.quat_accuracy; + timestamp[0] = dl_out.quat_timestamp; +} + +/** Gravity vector (gee) in body frame. +* @param[out] values Gravity vector in body frame, length 3, (gee) +* @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_accel(). +*/ +void inv_get_sensor_type_gravity_float(float *values, int *accuracy, + inv_time_t * timestamp) +{ + struct inv_single_sensor_t *pa = &dl_out.sc.accel; + + if (values) { + long lgravity[3]; + (void)inv_get_gravity(lgravity); + values[0] = (float) lgravity[0] / (1L << 16); + values[1] = (float) lgravity[1] / (1L << 16); + values[2] = (float) lgravity[2] / (1L << 16); + } + if (accuracy) + *accuracy = pa->accuracy; + if (timestamp) + *timestamp = pa->timestamp; +} + +/** +* This corresponds to Sensor.TYPE_ROTATION_VECTOR. +* The rotation vector represents the orientation of the device as a combination +* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ +* around an axis {x, y, z}. <br> +* The three elements of the rotation vector are +* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation +* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is +* equal to the direction of the axis of rotation. +* +* The three elements of the rotation vector are equal to the last three components of a unit quaternion +* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. +* +* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. +* The reference coordinate system is defined as a direct orthonormal basis, where: + + -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). + -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. + -Z points towards the sky and is perpendicular to the ground. +* @param[out] values +* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate +* @param[out] timestamp Timestamp. In (ns) for Android. +*/ +void inv_get_sensor_type_rotation_vector_float(float *values, int *accuracy, + inv_time_t * timestamp) +{ + if (accuracy) + *accuracy = dl_out.quat_accuracy; + if (timestamp) + *timestamp = dl_out.quat_timestamp; + if (values) { + if (dl_out.quat[0] >= 0) { + values[0] = dl_out.quat[1] * INV_TWO_POWER_NEG_30; + values[1] = dl_out.quat[2] * INV_TWO_POWER_NEG_30; + values[2] = dl_out.quat[3] * INV_TWO_POWER_NEG_30; + } else { + values[0] = -dl_out.quat[1] * INV_TWO_POWER_NEG_30; + values[1] = -dl_out.quat[2] * INV_TWO_POWER_NEG_30; + values[2] = -dl_out.quat[3] * INV_TWO_POWER_NEG_30; + } + } +} + +/** Main callback to generate HAL outputs. Typically not called by library users. */ +inv_error_t inv_generate_datalogger_outputs(struct inv_sensor_cal_t *sensor_cal) +{ + memcpy(&dl_out.sc, sensor_cal, sizeof(struct inv_sensor_cal_t)); + inv_get_quaternion_set(dl_out.quat, &dl_out.quat_accuracy, + &dl_out.quat_timestamp); + return INV_SUCCESS; +} + +/** Turns off generation of HAL outputs. */ +inv_error_t inv_stop_datalogger_outputs(void) +{ + return inv_unregister_data_cb(inv_generate_datalogger_outputs); +} + +/** Turns on generation of HAL outputs. This should be called after inv_stop_dl_outputs() +* to turn generation of HAL outputs back on. It is automatically called by inv_enable_dl_outputs().*/ +inv_error_t inv_start_datalogger_outputs(void) +{ + return inv_register_data_cb(inv_generate_datalogger_outputs, + INV_PRIORITY_HAL_OUTPUTS, INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); +} + +/** Initializes hal outputs class. This is called automatically by the +* enable function. It may be called any time the feature is enabled, but +* is typically not needed to be called by outside callers. +*/ +inv_error_t inv_init_datalogger_outputs(void) +{ + memset(&dl_out, 0, sizeof(dl_out)); + return INV_SUCCESS; +} + +/** Turns on creation and storage of HAL type results. +*/ +inv_error_t inv_enable_datalogger_outputs(void) +{ + inv_error_t result; + result = inv_init_datalogger_outputs(); + if (result) + return result; + return inv_register_mpl_start_notification(inv_start_datalogger_outputs); +} + +/** Turns off creation and storage of HAL type results. +*/ +inv_error_t inv_disable_datalogger_outputs(void) +{ + inv_stop_datalogger_outputs(); + return inv_unregister_mpl_start_notification(inv_start_datalogger_outputs); +} + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.h b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.h new file mode 100644 index 0000000..0e50faf --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/datalogger_outputs.h @@ -0,0 +1,61 @@ +/** + * @defgroup HAL_Outputs + * @brief Motion Library - HAL Outputs + * Sets up common outputs for HAL + * + * @{ + * @file datalogger_outputs.h + * @brief Windows 8 HAL outputs. + */ + +#ifndef _DATALOGGER_OUTPUTS_H_ +#define _DATALOGGER_OUTPUTS_H_ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* calibrated data */ +void inv_get_sensor_type_temperature_float(float *value, + inv_time_t *timestamp); +void inv_get_sensor_type_gyro_float(float *values, int8_t *accuracy, + inv_time_t *timestamp); +void inv_get_sensor_type_accel_float(float *values, int8_t *accuracy, + inv_time_t *timestamp); +void inv_get_sensor_type_compass_float(float *mag_north, float *true_north, + float *values, int8_t *accuracy, inv_time_t *timestamp); +void inv_get_sensor_type_quat_float(float *values, int *accuracy, + inv_time_t *timestamp); +void inv_get_sensor_type_gravity_float(float *values, int *accuracy, + inv_time_t * timestamp); +void inv_get_sensor_type_rotation_vector_float(float *values, int *accuracy, + inv_time_t * timestamp); + +/* uncalibrated data */ +void inv_get_sensor_type_gyro_raw_short(short *values, + inv_time_t *timestamp); +void inv_get_sensor_type_gyro_raw_body_float(float *values, + inv_time_t *timestamp); +void inv_get_sensor_type_accel_raw_short(short *values, + inv_time_t *timestamp); +void inv_get_sensor_type_compass_raw_short(short *values, + inv_time_t *timestamp); + +/* enabler/disabler APIs */ +inv_error_t inv_enable_datalogger_outputs(void); +inv_error_t inv_disable_datalogger_outputs(void); +inv_error_t inv_init_datalogger_outputs(void); +inv_error_t inv_start_datalogger_outputs(void); +inv_error_t inv_stop_datalogger_outputs(void); + +#ifdef __cplusplus +} +#endif + +#endif /* #ifndef _DATALOGGER_OUTPUTS_H_ */ + +/** + * @} + */ diff --git a/6515/libsensors_iio/software/simple_apps/playback/linux/main.c b/6515/libsensors_iio/software/simple_apps/playback/linux/main.c new file mode 100644 index 0000000..3eb64f0 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/playback/linux/main.c @@ -0,0 +1,898 @@ +/******************************************************************************* + * Copyright (c) 2012 InvenSense Corporation, All Rights Reserved. + ******************************************************************************/ + +/******************************************************************************* + * + * $Id: main.c 6146 2011-10-04 18:33:51Z jcalizo $ + * + ******************************************************************************/ + +#include <stdio.h> +#include <stdlib.h> +#include <time.h> + +#include "invensense.h" +#include "invensense_adv.h" +#include "and_constructor.h" +#include "ml_math_func.h" +#include "datalogger_outputs.h" + +#include "console_helper.h" + +#include "mlos.h" +#include "mlsl.h" + +#include "testsupport.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-playback" + +/* + Defines & Macros +*/ +#define UNPACK_3ELM_ARRAY(a) (a)[0], (a)[1], (a)[2] +#define UNPACK_4ELM_ARRAY(a) UNPACK_3ELM_ARRAY(a), (a)[3] +#define COMPONENT_NAME_MAX_LEN (30) +#define DEF_NAME(x) (#x) + +#define PRINT_ON_CONSOLE(...) \ + if (print_on_screen) \ + printf(__VA_ARGS__) +#define PRINT_ON_FILE(...) \ + if(stream_file) \ + fprintf(stream_file, __VA_ARGS__) + +#define PRINT(...) \ + PRINT_ON_CONSOLE(__VA_ARGS__); \ + PRINT_ON_FILE(__VA_ARGS__) +#define PRINT_FLOAT(width, prec, data) \ + PRINT_ON_CONSOLE("%+*.*f", \ + width, prec, data); \ + PRINT_ON_FILE("%+f", data) +#define PRINT_INT(width, data) \ + PRINT_ON_CONSOLE("%+*d", width, data); \ + PRINT_ON_FILE("%+d", data); +#define PRINT_LONG(width, data) \ + PRINT_ON_CONSOLE("%+*ld", width, data); \ + PRINT_ON_FILE("%+ld", data); + +#define PRINT_3ELM_ARRAY_FLOAT(w, p, data) \ + PRINT_FLOAT(w, p, data[0]); \ + PRINT(", "); \ + PRINT_FLOAT(w, p, data[1]); \ + PRINT(", "); \ + PRINT_FLOAT(w, p, data[2]); \ + PRINT(", "); +#define PRINT_4ELM_ARRAY_FLOAT(w, p, data) \ + PRINT_3ELM_ARRAY_FLOAT(w, p, data); \ + PRINT_FLOAT(w, p, data[3]); \ + PRINT(", "); + +#define PRINT_3ELM_ARRAY_LONG(w, data) \ + PRINT_LONG(w, data[0]); \ + PRINT(", "); \ + PRINT_LONG(w, data[1]); \ + PRINT(", "); \ + PRINT_LONG(w, data[2]); \ + PRINT(", "); +#define PRINT_4ELM_ARRAY_LONG(w, data) \ + PRINT_3ELM_ARRAY_LONG(w, data); \ + PRINT_LONG(w, data[3]); \ + PRINT(", "); + +#define PRINT_3ELM_ARRAY_INT(w, data) \ + PRINT_INT(w, data[0]); \ + PRINT(", "); \ + PRINT_INT(w, data[1]); \ + PRINT(", "); \ + PRINT_INT(w, data[2]); \ + PRINT(", "); +#define PRINT_4ELM_ARRAY_INT(w, data) \ + PRINT_3ELM_ARRAY_LONG(w, data); \ + PRINT_INT(w, data[3]); \ + PRINT(", "); + + +#define CASE_NAME(CODE) \ + case CODE: \ + return #CODE + +#define CALL_CHECK_N_PRINT(f) { \ + MPL_LOGI("\n"); \ + MPL_LOGI("################################################\n"); \ + MPL_LOGI("# %s\n", #f); \ + MPL_LOGI("################################################\n"); \ + MPL_LOGI("\n"); \ + CALL_N_CHECK(f); \ +} + +/* + Types +*/ +/* A badly named enum type to track state of user input for tracker menu. */ +typedef enum { + STATE_SELECT_A_TRACKER, + STATE_SET_TRACKER_STATE, /* I'm running out of ideas here. */ + STATE_COUNT +} user_state_t; + +/* bias trackers. */ +typedef enum { + BIAS_FROM_NO_MOTION, + FAST_NO_MOTION, + BIAS_FROM_GRAVITY, + BIAS_FROM_TEMPERATURE, + BIAS_FROM_LPF, + DEAD_ZONE, + NUM_TRACKERS +} bias_t; + +enum comp_ids { + TIME = 0, + CALIBRATED_GYROSCOPE, + CALIBRATED_ACCELEROMETER, + CALIBRATED_COMPASS, + RAW_GYROSCOPE, + RAW_GYROSCOPE_BODY, + RAW_ACCELEROMETER, + RAW_COMPASS, + QUATERNION_9_AXIS, + QUATERNION_6_AXIS, + GRAVITY, + HEADING, + COMPASS_BIAS_ERROR, + COMPASS_STATE, + TEMPERATURE, + TEMP_COMP_SLOPE, + LINEAR_ACCELERATION, + ROTATION_VECTOR, + MOTION_STATE, + + NUM_OF_IDS +}; + +struct component_list { + char name[COMPONENT_NAME_MAX_LEN]; + int order; +}; + +/* + Globals +*/ +static int print_on_screen = true; +static int one_time_print = true; +static FILE *stream_file = NULL; +static unsigned long sample_count = 0; +static int enabled_9x = true; + +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}; +signed char g_compass_orientation[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1}; + +#ifdef WIN32 +static double pc_freq; +static __int64 counter_start; +#else +static inv_time_t counter_start; +#endif + +struct component_list components[NUM_OF_IDS]; + +/* + Prototypes +*/ +void print_tracker_states(bias_t tracker); + +/* + Callbacks +*/ +/*--- motion / no motion callback function ---*/ +void check_motion_event(void) +{ + long msg = inv_get_message_level_0(1); + if (msg) { + if (msg & INV_MSG_MOTION_EVENT) { + MPL_LOGI("################################################\n"); + MPL_LOGI("## Motion\n"); + MPL_LOGI("################################################\n"); + } + if (msg & INV_MSG_NO_MOTION_EVENT) { + MPL_LOGI("################################################\n"); + MPL_LOGI("## No Motion\n"); + MPL_LOGI("################################################\n"); + } + } +} + +/* number to string coversion */ +char *compass_state_name(char* out, int state) +{ + switch(state) { + CASE_NAME(SF_NORMAL); + CASE_NAME(SF_DISTURBANCE); + CASE_NAME(SF_FAST_SETTLE); + CASE_NAME(SF_SLOW_SETTLE); + CASE_NAME(SF_STARTUP_SETTLE); + CASE_NAME(SF_UNCALIBRATED); + } + + #define UNKNOWN_ERROR_CODE 1234 + return ERROR_NAME(UNKNOWN_ERROR_CODE); +} + +/* component ID to name convertion */ +char *component_name(char *out, int comp_id) +{ + switch (comp_id) { + CASE_NAME(TIME); + CASE_NAME(CALIBRATED_GYROSCOPE); + CASE_NAME(CALIBRATED_ACCELEROMETER); + CASE_NAME(CALIBRATED_COMPASS); + CASE_NAME(RAW_GYROSCOPE); + CASE_NAME(RAW_GYROSCOPE_BODY); + CASE_NAME(RAW_ACCELEROMETER); + CASE_NAME(RAW_COMPASS); + CASE_NAME(QUATERNION_9_AXIS); + CASE_NAME(QUATERNION_6_AXIS); + CASE_NAME(GRAVITY); + CASE_NAME(HEADING); + CASE_NAME(COMPASS_BIAS_ERROR); + CASE_NAME(COMPASS_STATE); + CASE_NAME(TEMPERATURE); + CASE_NAME(TEMP_COMP_SLOPE); + CASE_NAME(LINEAR_ACCELERATION); + CASE_NAME(ROTATION_VECTOR); + CASE_NAME(MOTION_STATE); + } + + return "UNKNOWN"; +} + + +#ifdef WIN32 + +/* + Karthik Implementation. + http://stackoverflow.com/questions/1739259/how-to-use-queryperformancecounter +*/ +double get_counter(__int64 *counter_start, double *pc_freq) +{ + LARGE_INTEGER li; + double x; + QueryPerformanceCounter(&li); + x = (double) (li.QuadPart - (*counter_start)); + x = x / (*pc_freq); + return(x); +} + +void start_counter(double *pc_freq, __int64 *counter_start) +{ + LARGE_INTEGER li; + double x; + if(!QueryPerformanceFrequency(&li)) + printf("QueryPerformanceFrequency failed!\n"); + x = (double)(li.QuadPart); + *pc_freq = x / 1000.0; + QueryPerformanceCounter(&li); + *counter_start = li.QuadPart; +} + +#else + +unsigned long get_counter(void) +{ + return (inv_get_tick_count() - counter_start); +} + +void start_counter(void) +{ + counter_start = inv_get_tick_count(); +} + +#endif + +/* processed data callback */ +void fifo_callback(void) +{ + int print_on_screen_saved = print_on_screen; + int i; + + /* one_time_print causes the data labels to be printed on screen */ + if (one_time_print) { + print_on_screen = true; + } + for (i = 0; i < NUM_OF_IDS; i++) { + if (components[TIME].order == i) { + if (one_time_print) { + PRINT("TIME,"); + } else { +#ifdef WIN32 + double time_ms; + static int first_value = 0; + if(first_value == 0){ + first_value = 1; + start_counter(&pc_freq, &counter_start); + time_ms = 0; + } else { + time_ms = get_counter(&counter_start, &pc_freq); + } + PRINT("%6.0f, ", time_ms); +#else + unsigned long time_ms; + static int first_value = 0; + if(first_value == 0){ + first_value = 1; + start_counter(); + time_ms = 0; + } else { + time_ms = get_counter(); + } + PRINT("%6ld, ", time_ms); +#endif + } + } else if (components[CALIBRATED_GYROSCOPE].order == i) { + if (one_time_print) { + PRINT("CALIBRATED_GYROSCOPE_X," + "CALIBRATED_GYROSCOPE_Y," + "CALIBRATED_GYROSCOPE_Z,"); + /* + PRINT("CALIBRATED_GYROSCOPE_X_AVERAGE," + "CALIBRATED_GYROSCOPE_Y_AVERAGE," + "CALIBRATED_GYROSCOPE_Z_AVERAGE,"); + */ + } else { + /* + #define window 20 + static int cnt = 0; + static int valid = 0; + static float gyro_keep[window][3]; + int kk, jj; + float avg[3]; + */ + float gyro[3]; + inv_get_gyro_float(gyro); + PRINT_3ELM_ARRAY_FLOAT(10, 5, gyro); + PRINT(" "); + /* + memcpy(gyro_keep[cnt], gyro, sizeof(float) * 3); + cnt= (cnt + 1) % window; + if (cnt == window - 1) + valid = 1; + if (valid) { + memset(avg, 0, sizeof(float) * 3); + jj = (cnt + 1) % window; + for (kk = 0; kk < window; kk++) { + avg[0] += gyro_keep[jj][0] / window; + avg[1] += gyro_keep[jj][1] / window; + avg[2] += gyro_keep[jj][2] / window; + jj = (jj + 1) % window; + } + PRINT("%+f, %+f, %+f, ", + UNPACK_3ELM_ARRAY(avg)); + PRINT_3ELM_ARRAY_FLOAT(10, 5, avg); + PRINT(" "); + } + */ + } + } else if (components[CALIBRATED_ACCELEROMETER].order == i) { + if (one_time_print) { + PRINT("CALIBRATED_ACCELEROMETER_X," + "CALIBRATED_ACCELEROMETER_Y," + "CALIBRATED_ACCELEROMETER_Z,"); + } else { + float accel[3]; + inv_get_accel_float(accel); + PRINT_3ELM_ARRAY_FLOAT(10, 5, accel); + PRINT(" "); + } + } else if (components[CALIBRATED_COMPASS].order == i) { + if (one_time_print) { + PRINT("CALIBRATED_COMPASS_X," + "CALIBRATED_COMPASS_Y," + "CALIBRATED_COMPASS_Z,"); + } else { + long lcompass[3]; + float compass[3]; + inv_get_compass_set(lcompass, 0, 0); + compass[0] = inv_q16_to_float(lcompass[0]); + compass[1] = inv_q16_to_float(lcompass[1]); + compass[2] = inv_q16_to_float(lcompass[2]); + PRINT_3ELM_ARRAY_FLOAT(10, 5, compass); + PRINT(" "); + } + } else if (components[RAW_GYROSCOPE].order == i) { + if (one_time_print) { + PRINT("RAW_GYROSCOPE_X," + "RAW_GYROSCOPE_Y," + "RAW_GYROSCOPE_Z,"); + } else { + short raw[3]; + inv_get_sensor_type_gyro_raw_short(raw, NULL); + PRINT_3ELM_ARRAY_INT(6, raw); + PRINT(" "); + } + } else if (components[RAW_GYROSCOPE_BODY].order == i) { + if (one_time_print) { + PRINT("RAW_GYROSCOPE_BODY_X," + "RAW_GYROSCOPE_BODY_Y," + "RAW_GYROSCOPE_BODY_Z,"); + } else { + float raw_body[3]; + inv_get_sensor_type_gyro_raw_body_float(raw_body, NULL); + PRINT_3ELM_ARRAY_FLOAT(10, 5, raw_body); + PRINT(" "); + } + } else if (components[RAW_ACCELEROMETER].order == i) { + if (one_time_print) { + PRINT("RAW_ACCELEROMETER_X," + "RAW_ACCELEROMETER_Y," + "RAW_ACCELEROMETER_Z,"); + } else { + short raw[3]; + inv_get_sensor_type_accel_raw_short(raw, NULL); + PRINT_3ELM_ARRAY_INT(6, raw); + PRINT(" "); + } + } else if (components[RAW_COMPASS].order == i) { + if (one_time_print) { + PRINT("RAW_COMPASS_X," + "RAW_COMPASS_Y," + "RAW_COMPASS_Z,"); + } else { + short raw[3]; + inv_get_sensor_type_compass_raw_short(raw, NULL); + PRINT_3ELM_ARRAY_INT(6, raw); + PRINT(" "); + } + } else if (components[QUATERNION_9_AXIS].order == i) { + if (one_time_print) { + PRINT("QUATERNION_9_AXIS_X," + "QUATERNION_9_AXIS_Y," + "QUATERNION_9_AXIS_Z," + "QUATERNION_9_AXIS_w,"); + } else { + float quat[4]; + inv_get_quaternion_float(quat); + PRINT_4ELM_ARRAY_FLOAT(10, 5, quat); + PRINT(" "); + } + } else if (components[QUATERNION_6_AXIS].order == i) { + if (one_time_print) { + PRINT("QUATERNION_6_AXIS_X," + "QUATERNION_6_AXIS_Y," + "QUATERNION_6_AXIS_Z," + "QUATERNION_6_AXIS_w,"); + } else { + float quat[4]; + long temp[4]; + int j; + inv_time_t timestamp; + inv_get_6axis_quaternion(temp, ×tamp); + for (j = 0; j < 4; j++) + quat[j] = (float)temp[j] / (1 << 30); + PRINT_4ELM_ARRAY_FLOAT(10, 5, quat); + PRINT(" "); + } + } else if (components[HEADING].order == i) { + if (one_time_print) { + PRINT("HEADING,"); + } else { + float heading[1]; + inv_get_sensor_type_compass_float(heading, NULL, NULL, + NULL, NULL); + PRINT_FLOAT(10, 5, heading[0]); + PRINT(", "); + } + } else if (components[GRAVITY].order == i) { + if (one_time_print) { + PRINT("GRAVITY_X," + "GRAVITY_Y," + "GRAVITY_Z,"); + } else { + float gravity[3]; + inv_get_sensor_type_gravity_float(gravity, NULL, NULL); + PRINT_3ELM_ARRAY_FLOAT(10, 5, gravity); + PRINT(" "); + } + } else if (components[COMPASS_STATE].order == i) { + if (one_time_print) { + PRINT("COMPASS_STATE," + "GOT_COARSE_HEADING,"); + } else { + PRINT_INT(1, inv_get_compass_state()); + PRINT(", "); + PRINT_INT(1, inv_got_compass_bias()); + PRINT(", "); + } + } else if (components[COMPASS_BIAS_ERROR].order == i) { + if (one_time_print) { + PRINT("COMPASS_BIAS_ERROR_X," + "COMPASS_BIAS_ERROR_Y," + "COMPASS_BIAS_ERROR_Z,"); + } else { + long mbe[3]; + inv_get_compass_bias_error(mbe); + PRINT_3ELM_ARRAY_LONG(6, mbe); + } + } else if (components[TEMPERATURE].order == i) { + if (one_time_print) { + PRINT("TEMPERATURE,"); + } else { + float temp; + inv_get_sensor_type_temperature_float(&temp, NULL); + PRINT_FLOAT(8, 4, temp); + PRINT(", "); + } + } else if (components[TEMP_COMP_SLOPE].order == i) { + if (one_time_print) { + PRINT("TEMP_COMP_SLOPE_X," + "TEMP_COMP_SLOPE_Y," + "TEMP_COMP_SLOPE_Z,"); + } else { + long temp_slope[3]; + float temp_slope_f[3]; + (void)inv_get_gyro_ts(temp_slope); + temp_slope_f[0] = inv_q16_to_float(temp_slope[0]); + temp_slope_f[1] = inv_q16_to_float(temp_slope[1]); + temp_slope_f[2] = inv_q16_to_float(temp_slope[2]); + PRINT_3ELM_ARRAY_FLOAT(10, 5, temp_slope_f); + PRINT(" "); + } + } else if (components[LINEAR_ACCELERATION].order == i) { + if (one_time_print) { + PRINT("LINEAR_ACCELERATION_X," + "LINEAR_ACCELERATION_Y," + "LINEAR_ACCELERATION_Z,"); + } else { + float lin_acc[3]; + inv_get_linear_accel_float(lin_acc); + PRINT_3ELM_ARRAY_FLOAT(10, 5, lin_acc); + PRINT(" "); + } + } else if (components[ROTATION_VECTOR].order == i) { + if (one_time_print) { + PRINT("ROTATION_VECTOR_X," + "ROTATION_VECTOR_Y," + "ROTATION_VECTOR_Z,"); + } else { + float rot_vec[3]; + inv_get_sensor_type_rotation_vector_float(rot_vec, NULL, NULL); + PRINT_3ELM_ARRAY_FLOAT(10, 5, rot_vec); + PRINT(" "); + } + } else if (components[MOTION_STATE].order == i) { + if (one_time_print) { + PRINT("MOTION_STATE,"); + } else { + unsigned int counter; + PRINT_INT(1, inv_get_motion_state(&counter)); + PRINT(", "); + } + } else { + ; + } + } + PRINT("\n"); + + if (one_time_print) { + one_time_print = false; + print_on_screen = print_on_screen_saved; + } + sample_count++; +} + +void print_help(char *exename) +{ + printf( + "\n" + "Usage:\n" + "\t%s [options]\n" + "\n" + "Options:\n" + " [-h|--help] = shows this help\n" + " [-o|--output PREFIX] = to dump data on csv file whose file\n" + " prefix is specified by the parameter,\n" + " e.g. '<PREFIX>-<timestamp>.csv'\n" + " [-i|--input NAME] = to read the provided playback.bin file\n" + " [-c|--comp C] = enable the following components in the\n" + " given order:\n" + " t = TIME\n" + " T = TEMPERATURE,\n" + " s = TEMP_COMP_SLOPE,\n" + " G = CALIBRATED_GYROSCOPE,\n" + " A = CALIBRATED_ACCELEROMETER,\n" + " C = CALIBRATED_COMPASS,\n" + " g = RAW_GYROSCOPE,\n" + " b = RAW_GYROSCOPE_BODY,\n" + " a = RAW_ACCELEROMETER,\n" + " c = RAW_COMPASS,\n" + " Q = QUATERNION_9_AXIS,\n" + " 6 = QUATERNION_6_AXIS,\n" + " V = GRAVITY,\n" + " L = LINEAR_ACCELERATION,\n" + " R = ROTATION_VECTOR,\n" + " H = HEADING,\n" + " E = COMPASS_BIAS_ERROR,\n" + " S = COMPASS_STATE,\n" + " M = MOTION_STATE.\n" + "\n" + "Note on compass state values:\n" + " SF_NORMAL = 0\n" + " SF_DISTURBANCE = 1\n" + " SF_FAST_SETTLE = 2\n" + " SF_SLOW_SETTLE = 3\n" + " SF_STARTUP_SETTLE = 4\n" + " SF_UNCALIBRATED = 5\n" + "\n", + exename); +} + +char *output_filename_datetimestamp(char *out) +{ + time_t t; + struct tm *now; + t = time(NULL); + now = localtime(&t); + + sprintf(out, + "%02d%02d%02d_%02d%02d%02d.csv", + now->tm_year - 100, now->tm_mon + 1, now->tm_mday, + now->tm_hour, now->tm_min, now->tm_sec); + return out; +} + +int components_parser(char pname[], char requested[], int length) +{ + int j; + + /* forcibly disable all */ + for (j = 0; j < NUM_OF_IDS; j++) + components[j].order = -1; + + /* parse each character one a time */ + for (j = 0; j < length; j++) { + switch (requested[j]) { + case 'T': + components[TEMPERATURE].order = j; + break; + case 'G': + components[CALIBRATED_GYROSCOPE].order = j; + break; + case 'A': + components[CALIBRATED_ACCELEROMETER].order = j; + break; + case 'g': + components[RAW_GYROSCOPE].order = j; + break; + case 'b': + components[RAW_GYROSCOPE_BODY].order = j; + break; + case 'a': + components[RAW_ACCELEROMETER].order = j; + break; + case 'Q': + components[QUATERNION_9_AXIS].order = j; + break; + case '6': + components[QUATERNION_6_AXIS].order = j; + break; + case 'V': + components[GRAVITY].order = j; + break; + case 'L': + components[LINEAR_ACCELERATION].order = j; + break; + case 'R': + components[ROTATION_VECTOR].order = j; + break; + + /* these components don't need to be enabled */ + case 't': + components[TIME].order = j; + break; + case 'C': + components[CALIBRATED_COMPASS].order = j; + break; + case 'c': + components[RAW_COMPASS].order = j; + break; + case 'H': + components[HEADING].order = j; + break; + case 'E': + components[COMPASS_BIAS_ERROR].order = j; + break; + case 'S': + components[COMPASS_STATE].order = j; + break; + case 'M': + components[MOTION_STATE].order = j; + break; + + default: + MPL_LOGI("Error : unrecognized component '%c'\n", + requested[j]); + print_help(pname); + return 1; + break; + } + } + return 0; +} + +int main(int argc, char *argv[]) +{ +#ifndef INV_PLAYBACK_DBG + MPL_LOGI("Error : this application was not compiled with the " + "INV_PLAYBACK_DBG define and cannot work.\n"); + MPL_LOGI(" Recompile the libmllite libraries with #define " + "INV_PLAYBACK_DBG uncommented\n"); + MPL_LOGI(" in 'software/core/mllite/data_builder.h'.\n"); + return INV_ERROR; +#endif + + inv_time_t start_time; + double total_time; + char req_component_list[50] = "tQGACH"; + char input_filename[101] = "/data/playback.bin"; + int i = 0; + char *ver_str; + /* flags */ + int use_nm_detection = true; + + /* make sure there is no buffering of the print messages */ + setvbuf(stdout, NULL, _IONBF, 0); + + /* forcibly disable all and populate the names */ + for (i = 0; i < NUM_OF_IDS; i++) { + char str_tmp[COMPONENT_NAME_MAX_LEN]; + strcpy(components[i].name, component_name(str_tmp, i)); + components[i].order = -1; + } + + CALL_N_CHECK( inv_get_version(&ver_str) ); + MPL_LOGI("\n"); + MPL_LOGI("%s\n", ver_str); + MPL_LOGI("\n"); + + for (i = 1; i < argc; i++) { + if(strcmp(argv[i], "-h") == 0 + || strcmp(argv[i], "--help") == 0) { + print_help(argv[0]); + return INV_SUCCESS; + + } else if(strcmp(argv[i], "-o") == 0 + || strcmp(argv[i], "--output") == 0) { + char output_filename[200]; + char end[50] = ""; + i++; + + snprintf(output_filename, sizeof(output_filename), "%s-%s", + argv[i], output_filename_datetimestamp(end)); + stream_file = fopen(output_filename, "w+"); + if (!stream_file) { + printf("Unable to open file '%s'\n", output_filename); + return INV_ERROR; + } + MPL_LOGI("-- Output on file '%s'\n", output_filename); + + } else if(strcmp(argv[i], "-i") == 0 + || strcmp(argv[i], "--input") == 0) { + i++; + strncpy(input_filename, argv[i], sizeof(input_filename)); + MPL_LOGI("-- Playing back file '%s'\n", input_filename); + + } else if(strcmp(argv[i], "-n") == 0 + || strcmp(argv[i], "--nm") == 0) { + i++; + if (!strcmp(argv[i], "none")) { + use_nm_detection = 0; + } else if (!strcmp(argv[i], "regular")) { + use_nm_detection = 1; + } else if (!strcmp(argv[i], "fast")) { + use_nm_detection = 2; + } else { + MPL_LOGI("Error : unrecognized no-motion type '%s'\n", + argv[i]); + return INV_ERROR_INVALID_PARAMETER; + } + MPL_LOGI("-- No motion algorithm : '%s', %d\n", + argv[i], use_nm_detection); + + } else if(strcmp(argv[i], "-9") == 0 + || strcmp(argv[i], "--nine") == 0) { + MPL_LOGI("-- using 9 axis sensor fusion by default\n"); + enabled_9x = true; + + } else if(strcmp(argv[i], "-c") == 0 + || strcmp(argv[i], "--comp") == 0) { + i++; + strcpy(req_component_list, argv[i]); + + } else { + MPL_LOGI("Unrecognized command-line parameter '%s'\n", argv[i]); + return INV_ERROR_INVALID_PARAMETER; + } + } + + CALL_CHECK_N_RETURN_ERROR( + components_parser( + argv[0], + req_component_list, strlen(req_component_list))); + + /* set up callbacks */ + CALL_N_CHECK(inv_set_fifo_processed_callback(fifo_callback)); + + /* algorithm init */ + CALL_N_CHECK(inv_enable_quaternion()); + if (use_nm_detection == 1) { + CALL_N_CHECK(inv_enable_motion_no_motion()); + } else if (use_nm_detection == 2) { + CALL_N_CHECK(inv_enable_fast_nomot()); + } + CALL_N_CHECK(inv_enable_gyro_tc()); + CALL_N_CHECK(inv_enable_in_use_auto_calibration()); + CALL_N_CHECK(inv_enable_no_gyro_fusion()); + CALL_N_CHECK(inv_enable_results_holder()); + if (enabled_9x) { + CALL_N_CHECK(inv_enable_heading_from_gyro()); + CALL_N_CHECK(inv_enable_compass_bias_w_gyro()); + CALL_N_CHECK(inv_enable_vector_compass_cal()); + CALL_N_CHECK(inv_enable_9x_sensor_fusion()); + } + + CALL_CHECK_N_RETURN_ERROR(inv_enable_datalogger_outputs()); + CALL_CHECK_N_RETURN_ERROR(inv_constructor_start()); + + /* load persistent data */ + { + FILE *fc = fopen("invcal.bin", "rb"); + if (fc != NULL) { + size_t sz, rd; + inv_error_t result = 0; + // Read amount of memory we need to hold data + rd = fread(&sz, sizeof(size_t), 1, fc); + if (rd == sizeof(size_t)) { + unsigned char *cal_data = (unsigned char *)malloc(sizeof(sz)); + unsigned char *cal_ptr; + cal_ptr = cal_data; + *(size_t *)cal_ptr = sz; + cal_ptr += sizeof(sz); + /* read rest of the file */ + fread(cal_ptr, sz - sizeof(size_t), 1, fc); + //result = inv_load_mpl_states(cal_data, sz); + if (result) { + MPL_LOGE("Cal Load error\n"); + } + MPL_LOGI("inv_load_mpl_states()\n"); + free(cal_data); + } else { + MPL_LOGE("Cal Load error with size read\n"); + } + fclose(fc); + } + } + + sample_count = 0; + start_time = inv_get_tick_count(); + + /* playback data that was recorded */ + inv_set_playback_filename(input_filename, strlen(input_filename) + 1); + inv_set_debug_mode(RD_PLAYBACK); + CALL_N_CHECK(inv_playback()); + + total_time = (1.0 * inv_get_tick_count() - start_time) / 1000; + if (total_time > 0) { + MPL_LOGI("\nPlayed back %ld samples in %.2f s (%.1f Hz)\n", + sample_count, total_time , 1.0 * sample_count / total_time); + } + + if (stream_file) + fclose(stream_file); + + return INV_SUCCESS; +} + + diff --git a/6515/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/6515/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared Binary files differnew file mode 100755 index 0000000..f2c157d --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared diff --git a/6515/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk new file mode 100644 index 0000000..ed5fbf6 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk @@ -0,0 +1,98 @@ +EXEC = inv_self_test$(SHARED_APP_SUFFIX) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +APP_DIR = $(CURDIR)/../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +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 +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MPL_DIR) +CFLAGS += -I$(COMMON_DIR) +CFLAGS += -I$(HAL_DIR)/include +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += $(ANDROID_LINK_EXECUTABLE) + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) +INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") + $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/6515/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/self_test/build/filelist.mk new file mode 100644 index 0000000..492f47e --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/self_test/build/filelist.mk @@ -0,0 +1,11 @@ +#### filelist.mk for console_test #### + +# headers +HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h + +# sources +SOURCES := $(APP_DIR)/inv_self_test.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/6515/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/6515/libsensors_iio/software/simple_apps/self_test/inv_self_test.c new file mode 100644 index 0000000..e846499 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/self_test/inv_self_test.c @@ -0,0 +1,646 @@ +/** + * Self Test application for Invensense's MPU6xxx/MPU9xxx. + */ + +#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 "invensense.h" +#include "ml_math_func.h" +#include "storage_manager.h" +#include "ml_stored_data.h" +#include "ml_sysfs_helper.h" +#include "data_builder.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*)) +#define IIO_SYSFS_PATH "/sys/bus/iio/devices/iio:device0" +#define IIO_HUB_NAME "inv_hub" + +#define GYRO_PASS_STATUS_BIT 0x01 +#define ACCEL_PASS_STATUS_BIT 0x02 +#define COMPASS_PASS_STATUS_BIT 0x04 + +typedef union { + long l; + int i; +} bias_dtype; + +char *sysfs_names_ptr; +struct sysfs_attrbs { + char *name; + char *enable; + int enable_value; + char *power_state; + int power_state_value; + char *dmp_on; + int dmp_on_value; + char *self_test; + char *temperature; + char *gyro_enable; + int gyro_enable_value; + char *gyro_x_calibbias; + char *gyro_y_calibbias; + char *gyro_z_calibbias; + char *gyro_scale; + char *gyro_x_offset; + char *gyro_y_offset; + char *gyro_z_offset; + char *accel_enable; + int accel_enable_value; + char *accel_x_calibbias; + char *accel_y_calibbias; + char *accel_z_calibbias; + char *accel_scale; + char *accel_x_offset; + char *accel_y_offset; + char *accel_z_offset; + char *compass_enable; + int compass_enable_value; +} mpu; + +static struct inv_db_save_t save_data; +static bool write_biases_immediately = false; + +/** 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 */ +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 res=0; + FILE *fp; + + 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 android_hub(void) +{ + char dev_name[8]; + FILE *fp; + + fp = fopen(mpu.name, "r"); + fgets(dev_name, 8, fp); + fclose (fp); + + if (!strncmp(dev_name, IIO_HUB_NAME, sizeof(IIO_HUB_NAME))) + return true; + else + return false; +} + +int save_n_write_sysfs_int(char *filename, int data, int *old_value) +{ + int res; + res = read_sysfs_int(filename, old_value); + if (res < 0) { + return res; + } + //printf("saved %s = %d\n", filename, *old_value); + res = write_sysfs_int(filename, data); + if (res < 0) { + return res; + } + return 0; +} + +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"); + return -1; + } + + // Setup IIO sysfs path & build MPU's sysfs paths + sprintf(sysfs_path, "%s", IIO_SYSFS_PATH); + + sprintf(mpu.name, "%s%s", sysfs_path, "/name"); + 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_calibbias, "%s%s", + sysfs_path, "/in_anglvel_x_calibbias"); + sprintf(mpu.gyro_y_calibbias, "%s%s", + sysfs_path, "/in_anglvel_y_calibbias"); + sprintf(mpu.gyro_z_calibbias, "%s%s", + sysfs_path, "/in_anglvel_z_calibbias"); + sprintf(mpu.gyro_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale"); + sprintf(mpu.gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset"); + sprintf(mpu.gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset"); + sprintf(mpu.gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset"); + + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable"); + sprintf(mpu.accel_x_calibbias, "%s%s", sysfs_path, "/in_accel_x_calibbias"); + sprintf(mpu.accel_y_calibbias, "%s%s", sysfs_path, "/in_accel_y_calibbias"); + sprintf(mpu.accel_z_calibbias, "%s%s", sysfs_path, "/in_accel_z_calibbias"); + sprintf(mpu.accel_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale"); + sprintf(mpu.accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset"); + sprintf(mpu.accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset"); + sprintf(mpu.accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset"); + + 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++) { + printf("inv_self_test: sysfs path: %s\n", *dptr++); + } +#endif + return 0; +} + +void print_cal_file_content(struct inv_db_save_t *data) +{ + printf("------------------------------\n"); + printf("-- Calibration file content --\n"); + printf(" factory_gyro_bias[3] = %+ld %+ld %+ld\n", + data->factory_gyro_bias[0], data->factory_gyro_bias[1], + data->factory_gyro_bias[2]); + printf(" factory_accel_bias[3] = %+ld %+ld %+ld\n", + data->factory_accel_bias[0], data->factory_accel_bias[1], + data->factory_accel_bias[2]); + printf(" compass_bias[3] = %+ld %+ld %+ld\n", + data->compass_bias[0], data->compass_bias[1], data->compass_bias[2]); + printf(" gyro_temp = %+ld\n", data->gyro_temp); + printf(" gyro_bias_tc_set = %+d\n", data->gyro_bias_tc_set); + printf(" accel_temp = %+ld\n", data->accel_temp); + printf(" gyro_accuracy = %d\n", data->gyro_accuracy); + printf(" accel_accuracy = %d\n", data->accel_accuracy); + printf(" compass_accuracy = %d\n", data->compass_accuracy); + printf("------------------------------\n"); +} + +/******************************************************************************* + * M a i n + ******************************************************************************/ +int main(int argc, char **argv) +{ + FILE *fptr; + int self_test_status = 0; + inv_error_t result; + bias_dtype gyro_bias[3]; + bias_dtype gyro_scale; + bias_dtype accel_bias[3]; + bias_dtype accel_scale; + int scale_ratio; + size_t packet_sz; + int axis_sign = 1; + unsigned char *buffer; + long timestamp; + long long temperature = 0; + bool compass_present = true; + int c; + + result= inv_init_sysfs_attributes(); + if (result) + return -1; + + // Self-test for Non-Hub + inv_init_storage_manager(); + + // 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); + + // Self-test for Android Hub + if (android_hub() == true) { + fptr = fopen(mpu.self_test, "r"); + if (fptr) { + fscanf(fptr, "%d", &self_test_status); + printf("\nSelf-Test:Hub: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); + fclose(fptr); + result = 0; + } else { + printf("Hub-Self-Test:ERR-Couldn't invoke self-test\n"); + result = -1; + } + + free(sysfs_names_ptr); + return result; + } + + /* testing hook: if the command-line parameter is '-l' as in 'load', + the system will read out the MLCAL_FILE */ + while ((c = getopt(argc, argv, "lw")) != -1) { + switch (c) { + case 'l': + inv_get_mpl_state_size(&packet_sz); + + buffer = (unsigned char *)malloc(packet_sz + 10); + if (buffer == NULL) { + printf("Self-Test:Can't allocate buffer\n"); + return -1; + } + + fptr= fopen(MLCAL_FILE, "rb"); + if (!fptr) { + printf("Self-Test:ERR- Can't open cal file to read - %s\n", + MLCAL_FILE); + result = -1; + } + fread(buffer, 1, packet_sz, fptr); + fclose(fptr); + + result = inv_load_mpl_states(buffer, packet_sz); + if (result) { + printf("Self-Test:ERR - " + "Cannot load MPL states from cal file - error %d\n", + result); + free(buffer); + return -1; + } + free(buffer); + + print_cal_file_content(&save_data); + return 0; + break; + + case 'w': + write_biases_immediately = true; + break; + + case '?': + return -1; + } + } + + // Clear out data. + memset(&save_data, 0, sizeof(save_data)); + memset(gyro_bias,0, sizeof(gyro_bias)); + memset(accel_bias,0, sizeof(accel_bias)); + + // enable the device + if (save_n_write_sysfs_int(mpu.power_state, 1, + &mpu.power_state_value) < 0) { + printf("Self-Test:ERR-Failed to set power_state=1\n"); + } + if (save_n_write_sysfs_int(mpu.enable, 0, &mpu.enable_value) < 0) { + printf("Self-Test:ERR-Failed to disable master enable\n"); + } + if (save_n_write_sysfs_int(mpu.dmp_on, 0, &mpu.dmp_on_value) < 0) { + printf("Self-Test:ERR-Failed to disable DMP\n"); + } + if (save_n_write_sysfs_int(mpu.accel_enable, 1, + &mpu.accel_enable_value) < 0) { + printf("Self-Test:ERR-Failed to enable accel\n"); + } + if (save_n_write_sysfs_int(mpu.gyro_enable, 1, + &mpu.gyro_enable_value) < 0) { + printf("Self-Test:ERR-Failed to enable gyro\n"); + } + if (save_n_write_sysfs_int(mpu.compass_enable, 1, + &mpu.compass_enable_value) < 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_calibbias, &gyro_bias[0].i) < 0 || + read_sysfs_int(mpu.gyro_y_calibbias, &gyro_bias[1].i) < 0 || + read_sysfs_int(mpu.gyro_z_calibbias, &gyro_bias[2].i) < 0 || + read_sysfs_int(mpu.gyro_scale, &gyro_scale.i) < 0) { + memset(gyro_bias, 0, sizeof(gyro_bias)); + gyro_scale.i = 0; + printf("Self-Test:Failed to read Gyro bias\n"); + } else { + save_data.gyro_accuracy = 3; +#ifdef DEBUG_PRINT + printf("Self-Test:Gyro bias[0..2] = [%d %d %d]\n", + gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); +#endif + } + } else { + printf("Self-Test:Failed Gyro self-test\n"); + } + + if (self_test_status & ACCEL_PASS_STATUS_BIT) { + // Read Accel Bias + if (read_sysfs_int(mpu.accel_x_calibbias, &accel_bias[0].i) < 0 || + read_sysfs_int(mpu.accel_y_calibbias, &accel_bias[1].i) < 0 || + read_sysfs_int(mpu.accel_z_calibbias, &accel_bias[2].i) < 0 || + read_sysfs_int(mpu.accel_scale, &accel_scale.i) < 0) { + memset(accel_bias, 0, sizeof(accel_bias)); + accel_scale.i = 0; + printf("Self-Test:Failed to read Accel bias\n"); + } else { + save_data.accel_accuracy = 3; +#ifdef DEBUG_PRINT + printf("Self-Test:Accel bias[0..2] = [%d %d %d]\n", + accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); +#endif + } + } else { + printf("Self-Test:Failed Accel self-test\n"); + } + + if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) { + printf("Self-Test:Failed Gyro and Accel self-test, " + "nothing left to do\n"); + result = -1; + goto restore_settings; + } + + // Read temperature + fptr = fopen(mpu.temperature, "r"); + if (fptr != NULL) { + fscanf(fptr,"%lld %ld", &temperature, ×tamp); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read temperature\n"); + } + + /* + When we read gyro bias from sysfs, the bias is in the raw units scaled by + 1000 at the full scale used during self-test + (in_anglvel_self_test_scale). + We store the biases in raw units (+/- 2000 dps full scale assumed) + scaled by 2^16 therefore the gyro_bias[] have to be divided by the + ratio of 2000 / gyro_scale to comply. + */ + // TODO read this from the regular full scale in sysfs + scale_ratio = 2000 / gyro_scale.i; + save_data.factory_gyro_bias[0] = + (long)(gyro_bias[0].l / 1000.f * 65536.f / scale_ratio); + save_data.factory_gyro_bias[1] = + (long)(gyro_bias[1].l / 1000.f * 65536.f / scale_ratio); + save_data.factory_gyro_bias[2] = + (long)(gyro_bias[2].l / 1000.f * 65536.f / scale_ratio); + + // Save temperature @ time stored. + // Temperature is in degrees Celsius scaled by 2^16 + save_data.gyro_temp = temperature * (1L << 16); + save_data.gyro_bias_tc_set = true; + save_data.accel_temp = save_data.gyro_temp; + + // When we read accel bias, the bias is in raw units scaled by 1000. + // and it contains the gravity vector. + // Find the orientation of the device, by looking for gravity + int axis = 0; + if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) { + axis = 1; + } + if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) { + axis = 2; + } + if (accel_bias[axis].l < 0) { + axis_sign = -1; + } + + // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16 + /* + When we read accel bias from sysfs, the bias is in the raw units scaled + by 1000 at the full scale used during self-test + (in_accel_self_test_scale). + We store the biases in raw units (+/- 2 gee full scale assumed) + scaled by 2^16 therefore the accel_bias[] have to be multipled by the + ratio of accel_scale / 2 to comply. + */ + // TODO read this from the regular full scale in sysfs + scale_ratio = accel_scale.i / 2; + save_data.factory_accel_bias[0] = + (long)(accel_bias[0].l / 1000.f * 65536.f * scale_ratio); + save_data.factory_accel_bias[1] = + (long)(accel_bias[1].l / 1000.f * 65536.f * scale_ratio); + save_data.factory_accel_bias[2] = + (long)(accel_bias[2].l / 1000.f * 65536.f * scale_ratio); + +#ifdef DEBUG_PRINT + printf("Self-Test:Saved Accel bias[0..2] = [%ld %ld %ld]\n", + save_data.factory_accel_bias[0], save_data.factory_accel_bias[1], + save_data.factory_accel_bias[2]); +#endif + + /* + Remove gravity, gravity in raw units should be 16384 = 2^14 for a +/- + 2 gee setting. The data has been saved in Hw units scaled to 2^16, + so gravity needs to scale up accordingly. + */ + /* gravity correction for accel_bias format */ + long gravity = axis_sign * (32768L / accel_scale.i) * 1000L; + accel_bias[axis].l -= gravity; + /* gravity correction for saved_data.accel_bias format */ + gravity = axis_sign * (32768L / accel_scale.i) * 65536L; +#ifdef DEBUG_PRINT + printf("Self-Test:Gravity = %ld\n", gravity); +#endif + save_data.factory_accel_bias[axis] -= gravity; + + printf("Self-Test:Saved Accel bias (gravity removed) [0..2] = " + "[%ld %ld %ld]\n", + save_data.factory_accel_bias[0], save_data.factory_accel_bias[1], + save_data.factory_accel_bias[2]); + /* write the accel biases down to the hardware now */ + if (write_biases_immediately) { + int offsets[3] = {0}; + /* NOTE: 16 is the gee scale of the dmp_bias settings */ + scale_ratio = 16 / accel_scale.i; + /* NOTE: the 2 factor is to account the halved resolution for the + accel offset registers */ + offsets[0] = -accel_bias[0].l / 1000 / 2 / scale_ratio; + if (write_sysfs_int(mpu.accel_x_offset, offsets[0]) < 0) { + printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_x_offset); + } + offsets[1] = -accel_bias[1].l / 1000 / 2 / scale_ratio; + if (write_sysfs_int(mpu.accel_y_offset, offsets[1]) < 0) { + printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_y_offset); + } + offsets[2] = -accel_bias[2].l / 1000 / 2 / scale_ratio; + if (write_sysfs_int(mpu.accel_z_offset, offsets[2]) < 0) { + printf("Self-Test:ERR-Failed to write %s\n", mpu.accel_z_offset); + } + printf("Self-Test:Written Accel offsets[0..2] = [%d %d %d]\n", + offsets[0], offsets[1], offsets[2]); + } + + printf("Self-Test:Saved Gyro bias[0..2] = [%ld %ld %ld]\n", + save_data.factory_gyro_bias[0], save_data.factory_gyro_bias[1], + save_data.factory_gyro_bias[2]); + /* write the gyro biases down to the hardware now */ + if (write_biases_immediately) { + int offsets[3] = {0}; + /* NOTE: 1000 is the dps scale of the offset registers */ + scale_ratio = 1000 / gyro_scale.i; + offsets[0] = -gyro_bias[0].l / 1000 / scale_ratio; + if (write_sysfs_int(mpu.gyro_x_offset, offsets[0]) < 0) { + printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_x_offset); + } + offsets[1] = -gyro_bias[1].l / 1000 / scale_ratio; + if (write_sysfs_int(mpu.gyro_y_offset, offsets[1]) < 0) { + printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_y_offset); + } + offsets[2] = -gyro_bias[2].l / 1000 / scale_ratio; + if (write_sysfs_int(mpu.gyro_z_offset, offsets[2]) < 0) { + printf("Self-Test:ERR-Failed to write %s\n", mpu.gyro_z_offset); + } + printf("Self-Test:Written Gyro offsets[0..2] = [%d %d %d]\n", + offsets[0], offsets[1], offsets[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); + + // Get size of packet to store. + inv_get_mpl_state_size(&packet_sz); + + // Create place to store data + buffer = (unsigned char *)malloc(packet_sz + 10); + if (buffer == NULL) { + printf("Self-Test:Can't allocate buffer\n"); + result = -1; + goto free_sysfs_storage; + } + + // Store the data + result = inv_save_mpl_states(buffer, packet_sz); + if (result) { + 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", + MLCAL_FILE); + result= -1; + } + } + + free(buffer); + +restore_settings: + if (write_sysfs_int(mpu.dmp_on, mpu.dmp_on_value) < 0) { + printf("Self-Test:ERR-Failed to restore dmp_on\n"); + } + if (write_sysfs_int(mpu.accel_enable, mpu.accel_enable_value) < 0) { + printf("Self-Test:ERR-Failed to restore accel_enable\n"); + } + if (write_sysfs_int(mpu.gyro_enable, mpu.gyro_enable_value) < 0) { + printf("Self-Test:ERR-Failed to restore gyro_enable\n"); + } + if (compass_present) { + if (write_sysfs_int(mpu.compass_enable, mpu.compass_enable_value) < 0) { + printf("Self-Test:ERR-Failed to restore compass_enable\n"); + } + } + if (write_sysfs_int(mpu.enable, mpu.enable_value) < 0) { + printf("Self-Test:ERR-Failed to restore buffer/enable\n"); + } + if (write_sysfs_int(mpu.power_state, mpu.power_state_value) < 0) { + printf("Self-Test:ERR-Failed to restore power_state\n"); + } + +free_sysfs_storage: + free(sysfs_names_ptr); + return result; +} + diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/README b/6515/libsensors_iio/software/simple_apps/stress_iio/README new file mode 100644 index 0000000..eea7588 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/stress_iio/README @@ -0,0 +1,27 @@ +Usage of inv_stress_iio: + +> inv_stress_iio: + this will run enable and disable sequence sequentially. + In the enable sequence, this will enable all the engines, output, and all dmp + events. + In the disable sequence, this will disable all but accel engine; all outputs + are disabled, only events are allowed. + +> inv_stress_iio -e 10 -d 1: + this set the enable sequence to 10 seconds and disable sequence for 1 seconds. + +> inv_stress_iio -c: + this includes the compass in the enable sequence when you have 9150 or other + secondary bus compass. It won't work for compasses on the primary bus. + +> inv_stress_iio -r: + this set the enable and disable sequences to last a random time. It is good + for stress test. + +> inv_stress_iio -m: + this enables motion interrupt during the disable sequence. + When this is enabled, the driver enters low power accel mode and disables all + other sensor output (including quaternion, gyro, accel, and compass.) in the + disabled sequence: + + diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/build/android/shared.mk b/6515/libsensors_iio/software/simple_apps/stress_iio/build/android/shared.mk new file mode 100644 index 0000000..fe844a5 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/stress_iio/build/android/shared.mk @@ -0,0 +1,94 @@ +EXEC = inv_stress_iio$(SHARED_APP_SUFFIX) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +APP_DIR = $(CURDIR)/../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +MPL_DIR = $(INV_ROOT)/software/core/mpl + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MPL_DIR) +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += $(ANDROID_LINK_EXECUTABLE) + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) +INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(EXEC) with objects $(INV_OBJS_DST) $(PREBUILT_OBJS) and libraries $(INV_LIBS)\n") + $(LINK) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/build/filelist.mk b/6515/libsensors_iio/software/simple_apps/stress_iio/build/filelist.mk new file mode 100644 index 0000000..f201fbb --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/stress_iio/build/filelist.mk @@ -0,0 +1,12 @@ +#### filelist.mk for stress_iio #### + +# headers +#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h +HEADERS += $(APP_DIR)/iio_utils.h + +# sources +SOURCES := $(APP_DIR)/stress_iio.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/6515/libsensors_iio/software/simple_apps/stress_iio/iio_utils.h b/6515/libsensors_iio/software/simple_apps/stress_iio/iio_utils.h new file mode 100644 index 0000000..d62c573 --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/stress_iio/iio_utils.h @@ -0,0 +1,700 @@ +/* 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); + char temp[200]; + ret = 0; + if (temp == NULL) + return -ENOMEM; + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + printf("failed to open write %s\n", temp); + ret = -errno; + printf("ERROR1=%d\n", ret); + while(1); + goto error_free; + } + fprintf(sysfsfp, "%d", val); + fclose(sysfsfp); + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + printf("failed to open read %s\n", temp); + ret = -errno; + printf("ERROR2=%d\n", ret); + while(1); + 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: + fclose(sysfsfp); + //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) +{ + printf("echo %d > %s/%s\n", val, basedir, filename); + return _write_sysfs_int(filename, basedir, val, 1); +} + +inline int write_sysfs_int64(char *filename, char *basedir, long long val) +{ + int ret; + FILE *sysfsfp; + int test; + //char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + char temp[200]; + ret = 0; + if (temp == NULL) + return -ENOMEM; + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + printf("failed to open write %s\n", temp); + ret = -errno; + printf("ERROR1=%d\n", ret); + while(1); + goto error_free; + } + fprintf(sysfsfp, "%lld", val); + fclose(sysfsfp); +error_free: + fclose(sysfsfp); + //free(temp); + return ret; +} + +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; +} + +long long read_sysfs_poslonglong(char *filename, char *basedir) +{ + long long 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, "%lld\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/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c new file mode 100644 index 0000000..32bb7ad --- /dev/null +++ b/6515/libsensors_iio/software/simple_apps/stress_iio/stress_iio.c @@ -0,0 +1,1011 @@ +/* Industrialio buffer test code. + * + * Copyright (c) 2012 Invensense Inc. + * + * 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. + * + * Command line parameters + * stress_iio -d time1 -e time2 + */ + +#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 <pthread.h> +#include "iio_utils.h" +#include "ml_load_dmp.h" +#include "ml_sysfs_helper.h" +#include "authenticate.h" + +pthread_mutex_t data_switch_lock = PTHREAD_MUTEX_INITIALIZER; + +static int has_compass = 0; +static int has_pressure = 0; +static int enable_random_delay = 0; +static int enable_delay = 10; +static int disable_delay = 10; +static int enable_motion_on = 0; +static int final_output_rate; +static int first_flag; +static char dmp_path[200]; + +static int dev_num; +static char *dev_dir_name; +static char *buf_dir_name; +static char *scan_el_dir; +static int gyro_data_is_enabled, accel_data_is_enabled, compass_data_is_enabled, quaternion_data_is_enabled; +static int accel_engine_is_on; + +struct dmp_struct { + char fname[100]; + void (*action)(struct dmp_struct *, int); +}; + +static void HandleTap(struct dmp_struct *dmp, int tap); +static void sipmle_print(struct dmp_struct *dmp, int d){ + printf("%s:%d\n", dmp->fname, d); +} + +static void handle_smd() { + printf("write wake lock for SMD\n"); + //write_sysfs_string_and_verify("wake_lock", "/sys/power/", "hack"); +} + +static void pedo_print() +{ + printf("steps=%lld, time=%lld\n", + read_sysfs_poslonglong("pedometer_steps", dev_dir_name), + read_sysfs_poslonglong("pedometer_time", dev_dir_name)); +} + +struct dmp_struct event_file[] = { +#if 1 + { + .fname = "event_tap", + .action = HandleTap, + }, +#endif + { + .fname = "event_smd", + .action = handle_smd, + }, + { + .fname = "event_accel_motion", + .action = sipmle_print, + }, + { + .fname = "event_pedometer", + .action = pedo_print, + }, +}; + +static void HandleTap(struct dmp_struct *dmp, 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 2799 +static char dmp_img[DMP_CODE_SIZE]; +static void verify_img(){ + FILE *fp; + int i; + char dmp_path[] = "/sys/bus/iio/devices/iio:device0/dmp_firmware"; + + printf("saving image\n"); + 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, "unsigned char rec[]={\n"); + for(i=0; i<DMP_CODE_SIZE; i++) { + fprintf(fp, "0x%02x, ", dmp_img[i]); + //printf( "0x%02x, ", dmp_img[i]); + if(((i+1)%16) == 0) { + fprintf(fp, "\n"); + //printf("\n"); + } + } + fprintf(fp, "};\n "); + fclose(fp); + printf("saving image Done\n"); +} + +static void inv_set_rate() +{ + int ret; + + printf("set rate \n"); + ret = write_sysfs_int_and_verify("accel_rate", dev_dir_name, 5); + ret = write_sysfs_int_and_verify("gyro_rate", dev_dir_name, 5); + if (has_compass) + ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 50); + if (has_pressure) + ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 15); + ret = write_sysfs_int_and_verify("three_axes_q_rate", dev_dir_name, 5); +} + + +static int setup_offset_and_bias() +{ + int ret; + + ret = write_sysfs_int_and_verify("in_accel_x_offset", dev_dir_name, 0); + if (ret < 0) + printf("write accel x offset failed.\n"); + ret = write_sysfs_int_and_verify("in_accel_y_offset", dev_dir_name, 0); + if (ret < 0) + printf("write accel y offset failed.\n"); + ret = write_sysfs_int_and_verify("in_accel_z_offset", dev_dir_name, 0); + if (ret < 0) + printf("write accel z offset failed.\n"); + + ret = write_sysfs_int_and_verify("in_anglvel_x_offset", dev_dir_name, 0); + if (ret < 0) + printf("write accel x offset failed.\n"); + ret = write_sysfs_int_and_verify("in_anglvel_y_offset", dev_dir_name, 0); + if (ret < 0) + printf("write accel y offset failed.\n"); + ret = write_sysfs_int_and_verify("in_anglvel_z_offset", dev_dir_name, 0); + if (ret < 0) + printf("write accel z offset failed.\n"); + + ret = write_sysfs_int_and_verify("in_accel_x_dmp_bias", dev_dir_name, 0); + if (ret < 0) + printf("write accel x offset failed.\n"); + ret = write_sysfs_int_and_verify("in_accel_y_dmp_bias", dev_dir_name, 0); + if (ret < 0) + printf("write accel y offset failed.\n"); + ret = write_sysfs_int_and_verify("in_accel_z_dmp_bias", dev_dir_name, 0); + if (ret < 0) + printf("write accel z offset failed.\n"); + + ret = write_sysfs_int_and_verify("in_anglvel_x_dmp_bias", dev_dir_name, 0); + if (ret < 0) + printf("write gyro x offset failed.\n"); + ret = write_sysfs_int_and_verify("in_anglvel_y_dmp_bias", dev_dir_name, 0); + if (ret < 0) + printf("write gyro y offset failed.\n"); + ret = write_sysfs_int_and_verify("in_anglvel_z_dmp_bias", dev_dir_name, 0); + if (ret < 0) + printf("write gyro z offset failed.\n"); + + return 0; +} + +static void setup_dmp(char *dev_path){ + char sysfs_path[200]; + int ret; + FILE *fd; + sprintf(sysfs_path, "%s", dev_path); + printf("sysfs: %s\n", sysfs_path); + ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1); + if (ret < 0) + return; + + ret = write_sysfs_int("in_accel_scale", dev_path, 0); + if (ret < 0) + return; + ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); + 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("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_glu(sysfs_path, 0); + ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1); + ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1); + + ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1); + write_sysfs_int64("pedometer_steps", sysfs_path, 0x3ffffffff); + write_sysfs_int64("pedometer_time", sysfs_path, 0xffffffff); + if (ret < 0) + return; + + ret = setup_offset_and_bias(); + + return; +} + +#if 0 +static char reg_dump_arr[2000]; +static int inv_do_reg_dump(void) +{ + char reg_dump_name[100]; + int fd, i; + + sprintf(reg_dump_name, "%s/reg_dump", dev_dir_name); + printf("%s\n", reg_dump_name); + fd = open(reg_dump_name, O_RDONLY); + pread(fd, reg_dump_arr, 2000, 0); + close(fd); + for ( i = 0; i < 2000; i++) { + printf("%c", reg_dump_arr[i]); + //if((i+1)%16 == 0) + //printf("\n"); + } + return 0; +} +#endif + +static void *get_dmp_event(void *param) { + char file_name[100]; + int i; + int data; + char d[4]; + FILE *fp; + struct pollfd pfd[ARRAY_SIZE(event_file)]; + + printf("get DMP event: %s\n", dev_dir_name); + while(1) { + for (i = 0; i < ARRAY_SIZE(event_file); i++) { + sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname); + pfd[i].fd = open(file_name, O_RDONLY | O_NONBLOCK); + pfd[i].events = POLLPRI|POLLERR; + pfd[i].revents = 0; + read(pfd[i].fd, d, 4); + } + + poll(pfd, ARRAY_SIZE(event_file), -1); + for (i = 0; i < ARRAY_SIZE(event_file); i++) { + close(pfd[i].fd); + } + + for (i=0; i< ARRAY_SIZE(pfd); i++) { + if(pfd[i].revents != 0) { + sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname); + fp = fopen(file_name, "rt"); + fscanf(fp, "%d\n", &data); + event_file[i].action(&event_file[i], data); + } + } + } + + return 0; +} + +static int enable_gyro(int on){ + int ret; + ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, on); + if (ret < 0) + printf("write gyro_enable failed\n"); + + return ret; +} + +static int enable_gyro_output(int on){ + int ret; + gyro_data_is_enabled = on; + ret = write_sysfs_int_and_verify("gyro_fifo_enable", dev_dir_name, on); + if (ret < 0) + printf("write gyro_fifo_enable failed\n"); + + return ret; +} + +static int enable_compass(int on){ + int ret; + + compass_data_is_enabled = on; + ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, on); + if (ret < 0) + printf("write gyro_enable failed\n"); + + return ret; +} + +static int enable_pressure(int on){ + int ret; + + ret = write_sysfs_int_and_verify("pressure_enable", dev_dir_name, on); + if (ret < 0) + printf("write pressure_enable failed\n"); + + return ret; +} + +static int enable_quaternion(int on) { + int ret; + ret = write_sysfs_int_and_verify("ped_q_on", dev_dir_name, on); + if (ret < 0) + printf("write quaternion_on failed\n"); + ret = write_sysfs_int_and_verify("six_axes_q_on", dev_dir_name, on); + if (ret < 0) + printf("write quaternion_on failed\n"); + ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, on); + if (ret < 0) + printf("write quaternion_on failed\n"); + + return ret; +} +static int enable_step_detector(int on) { + int ret; + + ret = write_sysfs_int_and_verify("step_detector_on", dev_dir_name, on); + if (ret < 0) + printf("write step detector on failed\n"); +} +static int enable_step_indicator(int on) { + int ret; + + ret = write_sysfs_int_and_verify("step_indicator_on", dev_dir_name, on); + if (ret < 0) + printf("write step indicator on failed\n"); +} + +static int enable_accel(int on){ + int ret; + accel_data_is_enabled = on; + accel_engine_is_on = on; + ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, on); + if (ret < 0) + printf("write accel_enable failed\n"); + ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on); + if (ret < 0) + printf("write accel_fifo_enable failed\n"); + + return ret; +} +static int enable_accel_output(int on) { + int ret; + accel_data_is_enabled = on; + + ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on); + if (ret < 0) + printf("write accel_fifo_enable failed\n"); + + return ret; +} + +static int enable_enable(int on){ + int ret; + + if (0 == on) { + pthread_mutex_lock(&data_switch_lock); + } + ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, on); + if (ret < 0) + printf("write enable failed\n"); + + if (on) { + pthread_mutex_unlock(&data_switch_lock); + } + + return 0; +} +static int write_dmp_event(int on) { + int ret; + ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_dir_name, on); + if (ret < 0) + printf("write dmp_event_int_on failed\n"); + return 0; +} + +static void random_delay(){ + int i; + float bb; + + i = rand(); + bb = i * 200.0; + bb = i * 10.0; + i = 1 + (unsigned int)(bb/(RAND_MAX + 1.0)); + i *= 2; + if (i%2) { + printf("sleep %d ms\n", i); + usleep(i*1000); + } else { + printf("sleep %d s\n", i); + sleep(i); + } + +} +static void dmp_event_control(on){ + int ret; + + ret = 0; + +// ret = write_sysfs_int_and_verify("tap_on", dev_dir_name, on); + ret = write_sysfs_int_and_verify("smd_enable", dev_dir_name, 1); + if (ret < 0) + return; + inv_set_rate(); + + //ret = write_sysfs_int_and_verify("batchmode_wake_fifo_full_on", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 5000); +// ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0); + //ret = write_sysfs_int_and_verify("smd_delay_threshold", dev_dir_name, 10); + if (ret < 0) + return; + //ret = write_sysfs_int_and_verify("smd_threshold", dev_dir_name, 5000); + if (ret < 0) + return; + //write_sysfs_int_and_verify("motion_lpa_duration", dev_dir_name, 1000); + //write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, 200); + write_sysfs_int_and_verify("dmp_on", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("sampling_frequency", dev_dir_name, 200); + //write_sysfs_int_and_verify("motion_lpa_freq", dev_dir_name, 3); + +} +void enable_motion(int on) { + int ret; + + ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, on); + if (on) { + gyro_data_is_enabled = 0; + compass_data_is_enabled = 0; + quaternion_data_is_enabled = 0; + } +} +bool g, a; +static int counter = 0; +static unsigned char data_rate[] = {5, 10, 15, 50, 100, 200}; +static int run_enable_sequence() +{ + bool g, a, out; + + counter++; + g = rand()%2; + a = rand()%2; + if (!g && !a) + a = true; + + //g = true; + //a = true; + /*disable the master enable */ + enable_enable(0); + if(g) { + enable_gyro(1); + if (rand()%2) { + out = rand()%2; + enable_quaternion(out); + enable_gyro_output(!out); + } else { + enable_quaternion(1); + enable_gyro_output(1); + } + // enable_quaternion(0); + // enable_gyro_output(0); + + } else { + enable_gyro(0); + enable_gyro_output(0); + enable_quaternion(0); + } + if(a) { + enable_accel(1); + enable_accel_output(1); + } else { + enable_accel(0); + enable_accel_output(0); + } + if (has_compass) { + if(rand()%2) + enable_compass(1); + else + enable_compass(0); + enable_compass(counter%2); + //enable_compass(0); + } + if (has_pressure) { + if(rand()%2) + enable_pressure(1); + else + enable_pressure(0); + enable_pressure(counter%3); + //enable_pressure(0); + } + enable_step_detector(1); + enable_step_indicator(1); + //enable_step_detector(0); + //enable_step_indicator(0); + + write_dmp_event(0); + + enable_motion(0); + if (accel_engine_is_on) + dmp_event_control(1); + else + dmp_event_control(0); + first_flag = 1; + /*enable the master enable */ + enable_enable(1); + //write_sysfs_string_and_verify("wake_unlock", "/sys/power/", "hack"); + if (enable_random_delay) + random_delay(); + else { + printf("sleep %ds\n", enable_delay); + sleep(enable_delay); + } + + return 0; +} + +static int run_disable_sequence() { + enable_enable(0); + + enable_gyro(0); + enable_accel(1); + enable_quaternion(0); + enable_accel_output(0); + write_dmp_event(1); + enable_motion(enable_motion_on); + if (accel_engine_is_on) + dmp_event_control(1); + else + dmp_event_control(0); + + enable_enable(1); + if (enable_random_delay) + random_delay(); + else { + printf("sleep %ds\n", disable_delay); + sleep(disable_delay); + } + + return 0; +} +static int run_dmp_off() { + bool g, a, out; + + counter++; + g = rand()%2; + a = rand()%2; + if (!g && !a) + a = true; + + g = true; + a = true; +// a = false; +// g = false; + /*disable the master enable */ + enable_enable(0); + if(g) { + enable_gyro(1); + if (rand()%2) { + enable_gyro_output(!out); + } else { + enable_gyro_output(1); + } + enable_gyro_output(1); + + } else { + enable_gyro(0); + enable_gyro_output(0); + } + if(a) { + enable_accel(1); + enable_accel_output(1); +// enable_accel_output(0); + } else { + enable_accel(0); + enable_accel_output(0); + } + if (has_compass) { + if(rand()%2) + enable_compass(1); + else + enable_compass(0); + enable_compass(counter%2); + enable_compass(1); + } + if (has_pressure) { + if(rand()%2) + enable_pressure(1); + else + enable_pressure(0); + enable_pressure(counter%3); + enable_pressure(1); + } + + write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,100); + first_flag = 1; + /*enable the master enable */ + enable_enable(1); + sleep(2); + + return 0; +} +static void *control_switch(void *param) +{ + while(1) { + run_enable_sequence(); + run_dmp_off(); + printf("sleeping\n"); + sleep(1000); + run_disable_sequence(); + } + return 0; +} + +void get_sensor_data(char *d, short *sensor) +{ + int i; + + for (i = 0; i < 3; i++) + sensor[i] = *(short *)(d + 2 + i * 2); +} + +static void *read_data(void *param) +{ + char *buffer_access; + char data[1048], *dptr, tmp[24]; + short sensor[3]; + int q[3], i, ind, left_over_size, buf_size; + int ret, fp,read_size; + unsigned short hdr; + bool done_flag; + +#define PRESSURE_HDR 0x8000 +#define ACCEL_HDR 0x4000 +#define GYRO_HDR 0x2000 +#define COMPASS_HDR 0x1000 +#define LPQUAT_HDR 0x0800 +#define SIXQUAT_HDR 0x0400 +#define PEDQUAT_HDR 0x0200 +#define STEP_DETECTOR_HDR 0x0100 +#define STEP_INDICATOR_HDR 0x0001 +#define END_MARKER 0x0010 +#define EMPTY_MARKER 0x0020 + + printf("read_data Thread: %s\n", dev_dir_name); + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, dev_dir_name); + if (ret < 0) + goto error_alloc_scan_el_dir; + ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num); + if (ret < 0) + goto error_alloc_buffer_access; + + 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_open_buffer_access; + } + ind = 0; + + while(1) { + struct pollfd pfd = { + .fd = fp, + .events = POLLIN, + }; + poll(&pfd, 1, -1); + + if (left_over_size > 0) + memcpy(data, tmp, left_over_size); + dptr = data + left_over_size; + read_size = read(fp, dptr, 1024); + printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size); + if (read_size <= 0) { + printf("Wrong size=%d\n", read_size); + pthread_mutex_unlock(&data_switch_lock); + continue; + } + ind = read_size + left_over_size; + dptr = data; + printf("ind=%d\n", ind); + buf_size = ind - (dptr - data); + done_flag = false; + + while ((buf_size > 0) && (!done_flag)) { + hdr = *((short *)(dptr)); + if ((hdr & 0xf) && (hdr != STEP_INDICATOR_HDR)) + printf("STEP$$$$$$$$$$$$$$$=%x ", hdr); + switch (hdr & (~0xf)) { + case PRESSURE_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("PRESSURE:%d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case ACCEL_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("A:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case GYRO_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("G:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case COMPASS_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("M:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case PEDQUAT_HDR: + if (buf_size >= 16) { + get_sensor_data(dptr, sensor); + dptr += 8; + printf("PED:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); + } else + done_flag = true; + break; + case LPQUAT_HDR: + if (buf_size >= 24) { + q[0] = *(int *)(dptr + 4); + dptr += 8; + q[1] = *(int *)(dptr); + q[2] = *(int *)(dptr + 4); + dptr += 8; + printf("LPQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); + } else + done_flag = true; + break; + case SIXQUAT_HDR: + if (buf_size >= 24) { + q[0] = *(int *)(dptr + 4); + dptr += 8; + q[1] = *(int *)(dptr); + q[2] = *(int *)(dptr + 4); + dptr += 8; + printf("SIXQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); + } else + done_flag = true; + break; + case STEP_DETECTOR_HDR: + if (buf_size >= 16) { + printf("STEP DETECTOR "); + dptr += 8; + printf(" %lld\n", *(long long *)dptr); + } else + done_flag = true; + + break; + default: + if (hdr == EMPTY_MARKER) { + printf("emptry marker !!!!!!!!!!!\n"); + } else if (hdr == END_MARKER) { + printf("end marker !!!!!\n"); + } else { + dptr +=8; + printf("%lld\n", *(long long *)dptr); + } + break; + } + if (!done_flag) + dptr += 8; + buf_size = ind - (dptr - data); + } + if (ind - (dptr - data) > 0) + memcpy(tmp, dptr, ind - (dptr - data)); + left_over_size = ind - (dptr - data); + } + close(fp); + +error_open_buffer_access: + free(buffer_access); +error_alloc_buffer_access: + free(scan_el_dir); +error_alloc_scan_el_dir: + + return 0; +} + +static void inv_create_thread() { + pthread_t thread_dmp_event, thread_read_data, thread_control; + + pthread_create(&thread_dmp_event, NULL, &get_dmp_event, (void *)dev_dir_name); + pthread_create(&thread_read_data, NULL, &read_data, (void *)dev_dir_name); + pthread_create(&thread_control, NULL, &control_switch, (void *)dev_dir_name); + + pthread_join(thread_dmp_event, NULL); + pthread_join(thread_read_data, NULL); + pthread_join(thread_control, NULL); +} + +static int enable_enable_main(int on){ + int ret; + + printf("enable_enable: %s=%d\n", dev_dir_name, on); + ret = write_sysfs_int_and_verify("enable", buf_dir_name, on); + if (ret < 0) + printf("write enable failed\n"); + + return 0; +} + +int main(int argc, char **argv) +{ + unsigned long buf_len = 240; + + int ret, c, i; + + char *trigger_name = NULL; + + int datardytrigger = 1; + int trig_num; + char *dummy; + char chip_name[10]; + char device_name[10]; + char sysfs[100]; + + gyro_data_is_enabled = 0; + accel_data_is_enabled = 0; + compass_data_is_enabled = 0; + quaternion_data_is_enabled = 0; + + while ((c = getopt(argc, argv, "lcd:e:rmp")) != -1) { + switch (c) { + case 'c': + has_compass = 1; + break; + case 'p': + has_pressure = 1; + break; + case 'd': + disable_delay = strtoul(optarg, &dummy, 10); + break; + case 'e': + enable_delay = strtoul(optarg, &dummy, 10); + break; + case 'r': + enable_random_delay = 1; + break; + case 'm': + enable_motion_on = 1; + 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); + printf("allco=%x\n", (int)dev_dir_name); + 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; + } + } + /* 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); + ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_triggername; + } + enable_enable_main(0); + ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); + /* + * Parse the files in scan_elements to identify what channels are + * present + */ + ret = 0; + setup_dmp(dev_dir_name); + + 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; + enable_enable_main(1); + inv_create_thread(); +exit_here: +error_free_buf_dir_name: + free(buf_dir_name); +error_free_triggername: + if (datardytrigger) + free(trigger_name); +error_ret: + return ret; +} diff --git a/65xx/libsensors_iio/Android.mk b/65xx/libsensors_iio/Android.mk index 77d5ce3..349c961 100644 --- a/65xx/libsensors_iio/Android.mk +++ b/65xx/libsensors_iio/Android.mk @@ -30,8 +30,8 @@ LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" MAJOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f1 -d.) MINOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f2 -d.) VERSION_JB :=$(shell test $(MAJOR_VERSION) -gt 4 -o $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -gt 0 && echo true) -$(info MAJOR_VERSION=$(MAJOR_VERSION)) -$(info MINOR_VERSION=$(MINOR_VERSION)) +#$(info MAJOR_VERSION=$(MAJOR_VERSION)) +#$(info MINOR_VERSION=$(MINOR_VERSION)) #ANDROID version check END VERSION_JB:=true ifeq ($(VERSION_JB),true) @@ -1,8 +1,13 @@ # Can't have both 65xx and 60xx sensors. ifneq ($(filter hammerhead, $(TARGET_DEVICE)),) -# hammerhead expects 65xx sensors. -include $(call all-named-subdir-makefiles,65xx) + # hammerhead expects 65xx sensors. + include $(call all-named-subdir-makefiles,65xx) +else +ifneq ($(filter guppy dory platina, $(TARGET_DEVICE)),) +# dory, guppy, and platina expect 6515 sensors. +include $(call all-named-subdir-makefiles,6515) else # manta expects 60xx sensors. include $(call all-named-subdir-makefiles,60xx) endif +endif |