diff options
120 files changed, 19581 insertions, 0 deletions
diff --git a/Android.mk b/Android.mk new file mode 100644 index 0000000..427bbb4 --- /dev/null +++ b/Android.mk @@ -0,0 +1,8 @@ +# Can't have both manta and non-manta libsensors. +ifneq ($(filter manta, $(TARGET_DEVICE)),) +# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta. +include $(call all-named-subdir-makefiles,libsensors_iio) +else +# libsensors expects an non-IIO MPU3050. +include $(call all-named-subdir-makefiles,mlsdk libsensors) +endif diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk new file mode 100644 index 0000000..c3b2003 --- /dev/null +++ b/libsensors_iio/Android.mk @@ -0,0 +1,95 @@ +# 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_CFLAGS := -DLOG_TAG=\"Sensors\" + +ifeq ($(ENG_BUILD),1) +ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1) +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif +ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1) +LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL +endif +ifeq ($(COMPILE_COMPASS_YAS530),1) +LOCAL_CFLAGS += -DCOMPASS_YAS530 +endif +ifeq ($(COMPILE_COMPASS_AK8975),1) +LOCAL_CFLAGS += -DCOMPASS_AK8975 +endif +ifeq ($(COMPILE_COMPASS_AMI306),1) +LOCAL_CFLAGS += -DCOMPASS_AMI306 +endif +else # release builds, default +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif + +LOCAL_SRC_FILES := SensorBase.cpp +LOCAL_SRC_FILES += MPLSensor.cpp +LOCAL_SRC_FILES += MPLSupport.cpp +LOCAL_SRC_FILES += InputEventReader.cpp +LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp + +LOCAL_C_INCLUDES += $(LOCAL_PATH) +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include +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 + +include $(BUILD_SHARED_LIBRARY) + +include $(CLEAR_VARS) +LOCAL_MODULE := libmplmpu +LOCAL_SRC_FILES := libmplmpu.so +LOCAL_MODULE_TAGS := optional +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_SUFFIX := .so +LOCAL_MODULE_CLASS := SHARED_LIBRARIES +LOCAL_MODULE_PATH := $(TARGET_OUT)/lib +OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) +include $(BUILD_PREBUILT) + +endif # !TARGET_SIMULATOR diff --git a/libsensors_iio/CompassSensor.IIO.9150.cpp b/libsensors_iio/CompassSensor.IIO.9150.cpp new file mode 100644 index 0000000..ce0df34 --- /dev/null +++ b/libsensors_iio/CompassSensor.IIO.9150.cpp @@ -0,0 +1,421 @@ +/* + * 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" +#include "local_log_def.h" + +#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) + +#if defined COMPASS_YAS530 +# warning "Invensense compass cal with YAS530 on primary bus" +# define USE_MPL_COMPASS_HAL (1) +# define COMPASS_NAME "INV_YAS530" +#elif defined COMPASS_AK8975 +# warning "Invensense compass cal with AK8975 on primary bus" +# define USE_MPL_COMPASS_HAL (1) +# define COMPASS_NAME "INV_AK8975" +#elif defined INVENSENSE_COMPASS_CAL +# warning "Invensense compass cal with compass on secondary bus" +# define USE_MPL_COMPASS_HAL (1) +# define COMPASS_NAME "INV_COMPASS" +#else +# warning "third party compass cal HAL" +# define USE_MPL_COMPASS_HAL (0) +// TODO: change to vendor's name +# define COMPASS_NAME "AKM8975" +#endif + + +/*****************************************************************************/ + +CompassSensor::CompassSensor() + : SensorBase(NULL, NULL), + mCompassTimestamp(0), + mCompassInputReader(8), + compass_fd(-1) +{ + VFUNC_LOG; + + if(inv_init_sysfs_attributes()) { + LOGE("Error Instantiating Compass\n"); + return; + } + + if (!strcmp(COMPASS_NAME, "INV_COMPASS")) { + mI2CBus = COMPASS_BUS_SECONDARY; + } else { + mI2CBus = COMPASS_BUS_PRIMARY; + } + + 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]; + fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]); + fclose(fptr); + + LOGV_IF(EXTRA_VERBOSE, + "HAL:compass mounting matrix: " + "%+d %+d %+d %+d %+d %+d %+d %+d %+d", + om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); + + mCompassOrientation[0] = om[0]; + mCompassOrientation[1] = om[1]; + mCompassOrientation[2] = om[2]; + mCompassOrientation[3] = om[3]; + mCompassOrientation[4] = om[4]; + mCompassOrientation[5] = om[5]; + mCompassOrientation[6] = om[6]; + mCompassOrientation[7] = om[7]; + mCompassOrientation[8] = om[8]; + } else { + LOGE("HAL:Couldn't read compass mounting matrix"); + } + + enable(ID_M, 0); +} + +CompassSensor::~CompassSensor() +{ + VFUNC_LOG; + + free(pathP); + if( compass_fd > 0) + close(compass_fd); +} + +int CompassSensor::getFd() const +{ + VHANDLER_LOG; + return compass_fd; +} + +/** + * @brief This function will enable/disable sensor. + * @param[in] handle + * which sensor to enable/disable. + * @param[in] en + * en=1, enable; + * en=0, disable + * @return if the operation is successful. + */ +int CompassSensor::enable(int32_t handle, int en) +{ + VFUNC_LOG; + + mEnable = en; + int tempFd; + int res; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_enable, getTimestamp()); + tempFd = open(compassSysFs.compass_enable, O_RDWR); + res = errno; + if(tempFd < 0) { + LOGE("HAL:open of %s failed with '%s' (%d)", + compassSysFs.compass_enable, strerror(res), res); + return res; + } + res = enable_sysfs_sensor(tempFd, en); + LOGE_IF(res < 0, "HAL:enable compass failed"); + close(tempFd); + + if (en) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_x_fifo_enable, getTimestamp()); + tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + close(tempFd); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + compassSysFs.compass_x_fifo_enable, strerror(res), res); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_y_fifo_enable, getTimestamp()); + tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + close(tempFd); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + compassSysFs.compass_y_fifo_enable, strerror(res), res); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, compassSysFs.compass_z_fifo_enable, getTimestamp()); + tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + close(tempFd); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + compassSysFs.compass_z_fifo_enable, strerror(res), res); + } + } + + return res; +} + +int CompassSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + int tempFd; + int res; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp()); + mDelay = ns; + if (ns == 0) + return -1; + tempFd = open(compassSysFs.compass_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / ns); + if(res < 0) { + LOGE("HAL:Compass update delay error"); + } + return res; +} + + +/** + @brief This function will return the state of the sensor. + @return 1=enabled; 0=disabled +**/ +int CompassSensor::getEnable(int32_t handle) +{ + VFUNC_LOG; + return mEnable; +} + +/* use for Invensense compass calibration */ +#define COMPASS_EVENT_DEBUG (0) +void CompassSensor::processCompassEvent(const input_event *event) +{ + VHANDLER_LOG; + + switch (event->code) { + case EVENT_TYPE_ICOMPASS_X: + LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n"); + mCachedCompassData[0] = event->value; + break; + case EVENT_TYPE_ICOMPASS_Y: + LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n"); + mCachedCompassData[1] = event->value; + break; + case EVENT_TYPE_ICOMPASS_Z: + LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n"); + mCachedCompassData[2] = event->value; + break; + } + + mCompassTimestamp = + (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L; +} + +void CompassSensor::getOrientationMatrix(signed char *orient) +{ + VFUNC_LOG; + memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation)); +} + +long CompassSensor::getSensitivity() +{ + VFUNC_LOG; + + long sensitivity; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", + compassSysFs.compass_scale, getTimestamp()); + inv_read_data(compassSysFs.compass_scale, &sensitivity); + return sensitivity; +} + +/** + @brief This function is called by sensors_mpl.cpp + to read sensor data from the driver. + @param[out] data sensor data is stored in this variable. Scaled such that + 1 uT = 2^16 + @para[in] timestamp data's timestamp + @return 1, if 1 sample read, 0, if not, negative if error + */ +int CompassSensor::readSample(long *data, int64_t *timestamp) +{ + 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 = COMPASS_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, "INV_YAS530")) { + list->maxRange = COMPASS_AKM8975_RANGE; + list->resolution = COMPASS_AKM8975_RESOLUTION; + list->power = COMPASS_AKM8975_POWER; + list->minDelay = COMPASS_AKM8975_MINDELAY; + return; + } + if(!strcmp(compass, "INV_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], 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; + + do { + *dptr++ = 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_YAS530 || defined COMPASS_AK8975 + inv_get_input_number(COMPASS_NAME, &num); + tbuf[0] = num + 0x30; + tbuf[1] = 0; + sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf); +#if defined COMPASS_YAS530 + strcat(sysfs_path, "/yas530"); +#else + strcat(sysfs_path, "/ak8975"); +#endif + sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable"); + sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate"); + sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale"); + sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); +#else + sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable"); + 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"); +#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/libsensors_iio/CompassSensor.IIO.9150.h b/libsensors_iio/CompassSensor.IIO.9150.h new file mode 100644 index 0000000..6a51338 --- /dev/null +++ b/libsensors_iio/CompassSensor.IIO.9150.h @@ -0,0 +1,93 @@ +/* + * 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); + + // unnecessary for MPL + virtual int readEvents(sensors_event_t *data, int count) { return 0; } + + int readSample(long *data, int64_t *timestamp); + int providesCalibration() { return 0; } + void getOrientationMatrix(signed char *orient); + long getSensitivity(); + int getAccuracy() { return 0; } + void fillList(struct sensor_t *list); + int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); } + +private: + enum CompassBus { + COMPASS_BUS_PRIMARY = 0, + COMPASS_BUS_SECONDARY = 1 + } mI2CBus; + + struct sysfs_attrbs { + 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; + } 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/libsensors_iio/InputEventReader.cpp b/libsensors_iio/InputEventReader.cpp new file mode 100644 index 0000000..ca0a61a --- /dev/null +++ b/libsensors_iio/InputEventReader.cpp @@ -0,0 +1,105 @@ +/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define LOG_NDEBUG 0
+
+#include <stdint.h>
+#include <errno.h>
+#include <unistd.h>
+#include <poll.h>
+
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <cutils/log.h>
+
+#include "InputEventReader.h"
+#include "local_log_def.h"
+
+/*****************************************************************************/
+
+struct input_event;
+
+InputEventCircularReader::InputEventCircularReader(size_t numEvents)
+ : mBuffer(new input_event[numEvents * 2]),
+ mBufferEnd(mBuffer + numEvents),
+ mHead(mBuffer),
+ mCurr(mBuffer),
+ mFreeSpace(numEvents)
+{
+}
+
+InputEventCircularReader::~InputEventCircularReader()
+{
+ delete [] mBuffer;
+}
+
+#define INPUT_EVENT_DEBUG (0)
+ssize_t InputEventCircularReader::fill(int fd)
+{
+ size_t numEventsRead = 0;
+ LOGV_IF(INPUT_EVENT_DEBUG,
+ "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
+ if (mFreeSpace) {
+ const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
+ if (nread < 0 || nread % sizeof(input_event)) {
+ //LOGE("Partial event received nread=%d, required=%d",
+ // nread, sizeof(input_event));
+ //LOGE("FD trying to read is: %d");
+ // we got a partial event!!
+ if (INPUT_EVENT_DEBUG) {
+ LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n",
+ __PRETTY_FUNCTION__);
+ LOGV_IF(nread % sizeof(input_event),
+ "DEBUG:%s exit nread %% sizeof(input_event)\n",
+ __PRETTY_FUNCTION__);
+ }
+ return (nread < 0 ? -errno : -EINVAL);
+ }
+
+ numEventsRead = nread / sizeof(input_event);
+ if (numEventsRead) {
+ mHead += numEventsRead;
+ mFreeSpace -= numEventsRead;
+ if (mHead > mBufferEnd) {
+ size_t s = mHead - mBufferEnd;
+ memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
+ mHead = mBuffer + s;
+ }
+ }
+ }
+
+ LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);
+ return numEventsRead;
+}
+
+ssize_t InputEventCircularReader::readEvent(input_event const** events)
+{
+ *events = mCurr;
+ ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
+ return available ? 1 : 0;
+}
+
+void InputEventCircularReader::next()
+{
+ mCurr++;
+ mFreeSpace++;
+ if (mCurr >= mBufferEnd) {
+ mCurr = mBuffer;
+ }
+}
diff --git a/libsensors_iio/InputEventReader.h b/libsensors_iio/InputEventReader.h new file mode 100644 index 0000000..11c4e73 --- /dev/null +++ b/libsensors_iio/InputEventReader.h @@ -0,0 +1,47 @@ +/*
+* 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> + +/*****************************************************************************/ + +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; + +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/libsensors_iio/License.txt b/libsensors_iio/License.txt new file mode 100644 index 0000000..930f931 --- /dev/null +++ b/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/libsensors_iio/MPLSensor.cpp b/libsensors_iio/MPLSensor.cpp new file mode 100644 index 0000000..ae82459 --- /dev/null +++ b/libsensors_iio/MPLSensor.cpp @@ -0,0 +1,2555 @@ +/* +* 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 "MPLSensor.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 TESTING +// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */ + +#ifdef THIRD_PARTY_ACCEL +# warning "Third party accel" +# 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 misc. */ +/******************************************************************************/ +static int 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) + +static struct sensor_t sSensorList[] = +{ + {"MPL Gyroscope", "Invensense", 1, + SENSORS_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Accelerometer", "Invensense", 1, + SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"MPL Magnetic Field", "Invensense", 1, + SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, {}}, + {"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 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, {}}, +}; + +MPLSensor *MPLSensor::gMPLSensor = NULL; + +extern "C" { +void procData_cb_wrapper() +{ + if(MPLSensor::gMPLSensor) { + MPLSensor::gMPLSensor->cbProcData(); + } +} + +void setCallbackObject(MPLSensor* gbpt) +{ + MPLSensor::gMPLSensor = gbpt; +} + +MPLSensor* getCallbackObject() { + return MPLSensor::gMPLSensor; +} + +} // end of extern C + +#ifdef INV_PLAYBACK_DBG +static FILE *logfile = NULL; +#endif + +/******************************************************************************* + * MPLSensor class implementation + ******************************************************************************/ + +MPLSensor::MPLSensor(CompassSensor *compass) + : SensorBase(NULL, NULL), + mNewData(0), + mMasterSensorMask(INV_ALL_SENSORS), + mLocalSensorMask(ALL_MPL_SENSORS_NP), + mPollTime(-1), + mHaveGoodMpuCal(0), + mGyroAccuracy(0), + mAccelAccuracy(0), + mSampleCount(0), + mEnabled(0), + mOldEnabledMask(0), + mAccelInputReader(4), + mGyroInputReader(32), + mTempScale(0), + mTempOffset(0), + mTempCurrentTime(0), + mAccelScale(2), + mPendingMask(0), + mSensorMask(0), + mGyroOrientation{0}, + mAccelOrientation{0}, + mFeatureActiveMask(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); + +#ifdef INV_PLAYBACK_DBG + LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); + logfile = fopen("/data/playback.bin", "wb"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif + + /* 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(); + + /* turn on power state */ + onPower(1); + + /* reset driver master enable */ + masterEnable(0); + + /* Load DMP image if capable, ie. MPU6xxx/9xxx */ + // TODO: disabled for GED tablet +#ifdef ENABLE_LP_QUAT_FEAT + loadDMP(); +#endif + + /* open temperature fd for temp comp */ + LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature); + 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 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); + } + } + + /* 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[LinearAccel].version = sizeof(sensors_event_t); + mPendingEvents[LinearAccel].sensor = ID_LA; + mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; + mPendingEvents[LinearAccel].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Gravity].version = sizeof(sensors_event_t); + mPendingEvents[Gravity].sensor = ID_GR; + mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; + mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Gyro].version = sizeof(sensors_event_t); + mPendingEvents[Gyro].sensor = ID_GY; + mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; + mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); + mPendingEvents[Accelerometer].sensor = ID_A; + mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; + mPendingEvents[Accelerometer].acceleration.status + = SENSOR_STATUS_ACCURACY_HIGH; + + /* 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[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; + + mHandlers[RotationVector] = &MPLSensor::rvHandler; + mHandlers[LinearAccel] = &MPLSensor::laHandler; + mHandlers[Gravity] = &MPLSensor::gravHandler; + mHandlers[Gyro] = &MPLSensor::gyroHandler; + mHandlers[Accelerometer] = &MPLSensor::accelHandler; + mHandlers[MagneticField] = &MPLSensor::compassHandler; + mHandlers[Orientation] = &MPLSensor::orienHandler; + + for (int i = 0; i < numSensors; i++) { + mDelays[i] = 0; + } + + (void)inv_get_version(&ver_str); + LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str); + + /* setup MPL */ + inv_constructor_init(); + + /* load calibration file from /data/cal.bin */ + rv = inv_load_calibration(); + if(rv == INV_SUCCESS) + LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded"); + else + LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); + + inv_set_device_properties(); + + /* disable driver master enable the first sensor goes on */ + masterEnable(0); + enableGyro(0); + enableAccel(0); + enableCompass(0); + onPower(0); + +#ifdef INV_PLAYBACK_DBG + logfile = fopen("/data/playback.bin", "wb"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif +} + + +void MPLSensor::enable_iio_sysfs() { + VFUNC_LOG; + + int tempFd = 0; + char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN]; + FILE *tempFp = NULL; + + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:echo 1 > %s (%lld)", + mpu.in_timestamp_en, getTimestamp()); + tempFd = open(mpu.in_timestamp_en, O_RDWR); + if(tempFd < 0) { + LOGE("HAL:could not open timestamp enable"); + } else if(enable_sysfs_sensor(tempFd, 1) < 0) { + LOGE("HAL:could not enable timestamp"); + } + + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", + mpu.trigger_name, getTimestamp()); + tempFp = fopen(mpu.trigger_name, "r"); + if (tempFp == NULL) { + LOGE("HAL:could not open trigger name"); + } else if (fscanf(tempFp, "%s", iio_trigger_name) < 0) { + LOGE("HAL:could not read trigger name"); + } + fclose(tempFp); + + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:echo %s > %s (%lld)", + iio_trigger_name, mpu.current_trigger, getTimestamp()); + tempFp = fopen(mpu.current_trigger, "w"); + if (tempFp == NULL) { + LOGE("HAL:could not open current trigger"); + } else if (fprintf(tempFp, "%s", iio_trigger_name) < 0) { + LOGE("HAL:could not write current trigger"); + } + fclose(tempFp); + + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:echo %d > %s (%lld)", + IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp()); + tempFp = fopen(mpu.buffer_length, "w"); + if (tempFp == NULL) { + LOGE("HAL:could not open buffer length"); + } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) { + LOGE("HAL:could not write buffer length"); + } + fclose(tempFp); + + 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(PROCESS_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd); + } +} + +int MPLSensor::inv_constructor_init() +{ + 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() +{ + VFUNC_LOG; + + inv_error_t result; + + 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; + } + + // TODO: double-check for GED tablet + // result = inv_enable_motion_no_motion(); + 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(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled"); + result = inv_enable_vector_compass_cal(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + 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; + } + } + + result = inv_enable_9x_sensor_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_enable_no_gyro_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + // TODO: double-check for GED tablet + result = inv_enable_quat_accuracy_monitor(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/* TODO: create function pointers to calculate scale */ +void MPLSensor::inv_set_device_properties() +{ + 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, 2000L << 15); + + /* accel setup */ + orient = inv_orientation_matrix_to_scalar(mAccelOrientation); + // BMA250 + //inv_set_accel_orientation_and_scale(orient, 1LL << 22); + // MPU6050 + inv_set_accel_orientation_and_scale(orient, mAccelScale << 15); + + /* compass setup */ + signed char orientMtx[9]; + mCompassSensor->getOrientationMatrix(orientMtx); + orient = + inv_orientation_matrix_to_scalar(orientMtx); + long sensitivity; + sensitivity = mCompassSensor->getSensitivity(); + inv_set_compass_orientation_and_scale(orient, sensitivity); +} + +void MPLSensor::loadDMP() +{ + int res, fd; + FILE *fptr; + + if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) { + //DMP support only for MPU6xxx/9xxx currently + 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) { + LOGE("HAL:could not write to dmp"); + } else { + int res = inv_load_dmp(fptr); + if(res < 0) { + LOGE("HAL:load DMP failed"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); + } + fclose(fptr); + } + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded"); + } + } + + // onDMP(1); //Can't enable here. See note onDMP() +} + +void MPLSensor::inv_get_sensors_orientation() +{ + 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]; + fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]); + fclose(fptr); + + LOGV_IF(EXTRA_VERBOSE, + "HAL: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]; + } else { + LOGE("HAL:Couldn't read gyro mounting matrix"); + } + + // 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]; + fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]); + fclose(fptr); + + LOGV_IF(EXTRA_VERBOSE, + "HAL: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]; + } else { + LOGE("HAL:Couldn't read accel mounting matrix"); + } +} + +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); + + /* Turn off Gyro master enable */ + /* A workaround until driver handles it */ + /* TODO: Turn off and close all sensors */ + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp()); + int fd = open(mpu.chip_enable, O_RDWR); + if(fd < 0) { + LOGE("HAL:could not open gyro chip enable"); + } else { + if(enable_sysfs_sensor(fd, 0) < 0) { + LOGE("HAL:could not disable gyro master enable"); + } + } + +#ifdef INV_PLAYBACK_DBG + inv_turn_off_data_logging(); + fclose(logfile); +#endif +} + +#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) // ID_GY = 0 +#define A_ENABLED ((1 << ID_A) & enabled_sensors) // ID_A = 1 +#ifdef INVENSENSE_COMPASS_CAL // ID_M = 2 +#define M_ENABLED ((1 << ID_M) & enabled_sensors) +#else +// TODO: ID_M = 2 even for 3rd-party solution +#define M_ENABLED ((1 << ID_M) & enabled_sensors) +#endif +#define O_ENABLED ((1 << ID_O) & enabled_sensors) // ID_O = 3 +#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4 +#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5 +#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6 + +/* TODO: this step is optional, remove? */ +int MPLSensor::setGyroInitialState() +{ + VFUNC_LOG; + + int res = 0; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp()); + int fd = open(mpu.gyro_fifo_rate, O_RDWR); + res = errno; + if(fd < 0) { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.gyro_fifo_rate, strerror(res), res); + return res; + } + res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ); + if(res < 0) { + LOGE("HAL:write_attribute_sensor : error writing %s with %d", + mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ); + return res; + } + + // Setting LPF is deprecated + + return 0; +} + +/* this applies to BMA250 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::onPower(int en) +{ + VFUNC_LOG; + + int res = 0; + char buf[sizeof(int)+1]; + int count, curr_power_state; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.power_state, getTimestamp()); + int tempFd = open(mpu.power_state, O_RDWR); + res = errno; + if(tempFd < 0){ + LOGE("HAL:Open of %s failed with '%s' (%d)", + mpu.power_state, strerror(res), res); + } else { + // check and set new power state + count = read_attribute_sensor(tempFd, buf, sizeof(buf)); + if(count < 1) { + LOGE("HAL:Error reading power state"); + // will set power_state anyway + curr_power_state= -1; + } else { + sscanf(buf, "%d", &curr_power_state); + } + + if (en!=curr_power_state) { + if((res=enable_sysfs_sensor(tempFd, en)) < 0) { + LOGE("HAL:Couldn't write power state"); + } + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:Power state already enable/disable curr=%d new=%d", + curr_power_state, en); + close(tempFd); + } + } + return res; +} + +int MPLSensor::onDMP(int en) +{ + VFUNC_LOG; + + int res= -1; + int status; + + //Sequence to enable DMP + //1. Turn On power if not already on + //2. Load DMP image if not already loaded + //3. Either Gyro or Accel must be enabled/configured before next step + //4. Enable DMP + + 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 + if (read_sysfs_int(mpu.dmp_on, &status) < 0) { + LOGE("HAL:ERR can't read DMP state"); + } else if (status != en) { + if (write_sysfs_int(mpu.dmp_on, en) < 0) { + LOGE("HAL:ERR can't write dmp_on"); + } else { + res= 0; //Indicate write successful + } + //Enable DMP interrupt + if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { + LOGE("HAL:ERR can't en/dis DMP interrupt"); + } + } else { + res= 0; //DMP already set as requested + } + } else { + LOGE("HAL:ERR No DMP image"); + } + return res; +} + +int MPLSensor::checkLPQuaternion(void) +{ + VFUNC_LOG; + + return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); +} + +int MPLSensor::enableLPQuaternion(int en) +{ + VFUNC_LOG; + + if (!en) { + enableQuaternionData(0); + onDMP(0); + mFeatureActiveMask &= ~INV_DMP_QUATERNION; + LOGV_IF(PROCESS_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(PROCESS_VERBOSE, "HAL:LP Quat enabled"); + } + } + return 0; +} + +int MPLSensor::enableQuaternionData(int en) +{ + int res= 0; + VFUNC_LOG; + + //Enable DMP quaternion + if (write_sysfs_int(mpu.quaternion_on, en) < 0) { + LOGE("HAL:ERR can't write DMP quaternion_on"); + res= -1; //Indicate an err + } + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off"); + inv_quaternion_sensor_was_turned_off(); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems"); + if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) { + LOGE("HAL:ERR write in_quat_r_en"); + } + if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) { + LOGE("HAL:ERR write in_quat_x_en"); + } + if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) { + LOGE("HAL:ERR write in_quat_y_en"); + } + if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) { + LOGE("HAL:ERR write in_quat_z_en"); + } + } + + return res; + +} + +int MPLSensor::enableTap(int en) +{ + VFUNC_LOG; + + return 0; +} + +int MPLSensor::enableFlick(int en) +{ + VFUNC_LOG; + + return 0; +} + +int MPLSensor::enablePedometer(int en) +{ + VFUNC_LOG; + + return 0; +} + +int MPLSensor::masterEnable(int en) +{ + VFUNC_LOG; + + int res = 0; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.chip_enable, getTimestamp()); + int tempFd = open(mpu.chip_enable, O_RDWR); + res = errno; + if(tempFd < 0){ + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.chip_enable, strerror(res), res); + return res; + } + res = enable_sysfs_sensor(tempFd, 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()); + int tempFd = open(mpu.gyro_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.gyro_enable, strerror(res), res); + } + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); + inv_gyro_was_turned_off(); + } else { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.gyro_x_fifo_enable, getTimestamp()); + tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.gyro_x_fifo_enable, strerror(res), res); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.gyro_y_fifo_enable, getTimestamp()); + tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.gyro_y_fifo_enable, strerror(res), res); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.gyro_z_fifo_enable, getTimestamp()); + tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.gyro_z_fifo_enable, strerror(res), res); + } + } + + 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()); + int tempFd = open(mpu.accel_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.accel_enable, strerror(res), res); + } + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); + inv_accel_was_turned_off(); + } else { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.accel_x_fifo_enable, getTimestamp()); + tempFd = open(mpu.accel_x_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.accel_x_fifo_enable, strerror(res), res); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.accel_y_fifo_enable, getTimestamp()); + tempFd = open(mpu.accel_y_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.accel_y_fifo_enable, strerror(res), res); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.accel_z_fifo_enable, getTimestamp()); + tempFd = open(mpu.accel_z_fifo_enable, O_RDWR); + res = errno; + if (tempFd > 0) { + res = enable_sysfs_sensor(tempFd, en); + } else { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.accel_z_fifo_enable, strerror(res), res); + } + } + + if (!en && USE_THIRD_PARTY_ACCEL == 0) { + } + + if(USE_THIRD_PARTY_ACCEL == 1 && en) { + setAccelInitialState(); // BMA250 + } + return res; +} + +int MPLSensor::enableCompass(int en) +{ + VFUNC_LOG; + + int res = mCompassSensor->enable(ID_M, en); + if (en == 0) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off"); + inv_compass_was_turned_off(); + } + return res; +} + +void MPLSensor::computeLocalSensorMask(int enabled_sensors) +{ + VFUNC_LOG; + + do { + if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { + LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); + mLocalSensorMask = ALL_MPL_SENSORS_NP; + break; + } + + if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) { + /* Invensense compass cal */ + LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); + mLocalSensorMask = 0; + break; + } + + if (GY_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) { + 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; + + // Sequence to enable or disable a sensor + // 1. enable Power state + // 2. reset master enable (=0) + // 3. enable or disable a sensor + // 4. set master enable (=1) + + if (changed & ((1 << Gyro) | (1 << Accelerometer) | + (mCompassSensor->isIntegrated() << MagneticField))) { + /* ensure power state is on */ + onPower(1); + + /* reset master enable */ + res = masterEnable(0); + if(res < 0) { + return res; + } + } + + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors); + + if (changed & (1 << Gyro)) { + if(sensors & INV_THREE_AXIS_GYRO) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro"); + res = enableGyro(on); + if(res < 0) { + return res; + } + } else if((sensors & INV_THREE_AXIS_GYRO) == 0) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro"); + res = enableGyro(off); + if(res < 0) { + return res; + } + } + } + + if (changed & (1 << Accelerometer)) { + if(sensors & INV_THREE_AXIS_ACCEL) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel"); + res = enableAccel(on); + if(res < 0) { + return res; + } + } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel"); + res = enableAccel(off); + if(res < 0) { + return res; + } + } + } + + if (changed & (1 << MagneticField)) { + /* Invensense compass calibration */ + if (sensors & INV_THREE_AXIS_COMPASS) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable compass"); + res = enableCompass(on); + if(res < 0) { + return res; + } + } else if ((sensors & INV_THREE_AXIS_COMPASS) == 0) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable compass"); + res = enableCompass(off); + if(res < 0) { + return res; + } + } + } + +// TODO: disabled for GED tablet +#ifdef ENABLE_LP_QUAT_FEAT + // Enable LP Quat + if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity)))) { + if (!checkLPQuaternion()) { + enableLPQuaternion(1); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled"); + } + } else if (checkLPQuaternion()) { + enableLPQuaternion(0); + } +#endif + + /* + if sensor & THREE_AXIS_GYRO + enable = 1 + if sensor & THREE_AXIS_ACCEL + enable = 1 + if compass_on_secondary + if sensor & THREE_AXIS_COMPASS + enable = 1 + else + enable = 0 + */ + if (changed & ((1 << Gyro) | (1 << Accelerometer) | + (mCompassSensor->isIntegrated() << MagneticField))) { + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { + res = masterEnable(1); + if(res < 0) { + return res; + } + } else { // all sensors idle -> reduce power + res = onPower(0); + if(res < 0) { + return res; + } + storeCalibration(); + } + } + + return res; +} + +/* Store calibration file */ +void MPLSensor::storeCalibration() +{ + if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) { + int res = inv_store_calibration(); + if (res) { + LOGE("HAL:Cannot store calibration on file"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated"); + } + } +} + +void MPLSensor::cbProcData() +{ + mNewData = 1; + mSampleCount++; + LOGV_IF(EXTRA_VERBOSE, "HAL:new data"); +} + +/* 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::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); + return update; +} + +int MPLSensor::rvHandler(sensors_event_t* s) +{ + // rotation vector does not have an accuracy or status + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d", + s->data[0], s->data[1], s->data[2], s->data[3], 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); + 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); + 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); + 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::enable(int32_t handle, int en) +{ + VFUNC_LOG; + + android::String8 sname; + int what = -1; + + switch (handle) { + case ID_A: + what = Accelerometer; + sname = "Accelerometer"; + break; + case ID_M: + what = MagneticField; + sname = "MagneticField"; + break; + case ID_O: + what = Orientation; + sname = "Orientation"; + break; + case ID_GY: + what = Gyro; + sname = "Gyro"; + break; + case ID_GR: + what = Gravity; + sname = "Gravity"; + break; + case ID_RV: + what = RotationVector; + sname = "RotationVector"; + break; + case ID_LA: + what = LinearAccel; + sname = "LinearAccel"; + break; + default: //this takes care of all the gestures + what = handle; + sname = "Others"; + break; + } + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + int newState = en ? 1 : 0; + int err = 0; + unsigned long sen_mask; + + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle, + ((mEnabled & (1 << what)) ? "en" : "dis"), + ((uint32_t(newState) << what) ? "en" : "dis")); + LOGV_IF(PROCESS_VERBOSE, + "HAL:%s sensor state change what=%d", sname.string(), what); + + // TODO: disabled for GED tablet + // pthread_mutex_lock(&mMplMutex); + // pthread_mutex_lock(&mHALMutex); + + 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(PROCESS_VERBOSE, "HAL:handle = %d", handle); + LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags); + computeLocalSensorMask(mEnabled); + LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled); + sen_mask = mLocalSensorMask & mMasterSensorMask; + mSensorMask = sen_mask; + LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask); + + switch (what) { + case Gyro: + case Accelerometer: + case MagneticField: + if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity))) && + ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { + changed |= (1 << what); + } + break; + + case Orientation: + case RotationVector: + case LinearAccel: + case Gravity: + if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity)))) || + (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity))))) { + for (int i = Gyro; i <= MagneticField; i++) { + if (!(mEnabled & (1 << i))) { + changed |= (1 << i); + } + } + } + break; + } + LOGV_IF(PROCESS_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 be go through this sequence + to properly flush the log file */ + inv_turn_off_data_logging(); + fclose(logfile); + logfile = fopen("/data/playback.bin", "ab"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif + + return err; +} + +int MPLSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + + android::String8 sname; + int what = -1; + + switch (handle) { + case ID_A: + what = Accelerometer; + sname = "Accelerometer"; + break; + case ID_M: + what = MagneticField; + sname = "MagneticField"; + break; + case ID_O: + what = Orientation; + sname = "Orientation"; + break; + case ID_GY: + what = Gyro; + sname = "Gyro"; + break; + case ID_GR: + what = Gravity; + sname = "Gravity"; + break; + case ID_RV: + what = RotationVector; + sname = "RotationVector"; + break; + case ID_LA: + what = LinearAccel; + sname = "LinearAccel"; + break; + default: // this takes care of all the gestures + what = handle; + sname = "Others"; + break; + } + +// TODO: disabled for GED tablet +#if 0 + // skip the 1st call for enalbing sensors called by ICS/JB sensor service + static int counter_delay = 0; + if (!(mEnabled & (1 << what))) { + counter_delay = 0; + } else { + if (++counter_delay == 1) { + return 0; + } + else { + counter_delay = 0; + } + } +#endif + + 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); + + // TODO: for GED tablet + // limit all rates to reasonable ones */ +/* + if (ns < 10000000LL) { + ns = 10000000LL; + } +*/ + if (ns < 5000000LL) { + ns = 5000000LL; + } + + /* store request rate to mDelays arrary for each sensor */ + mDelays[what] = ns; + + switch (what) { + case Gyro: + case Accelerometer: + for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); + i++) { + if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { + LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); + return 0; + } + } + break; + + case MagneticField: + if (mCompassSensor->isIntegrated() && + (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || + ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) { + LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel"); + return 0; + } + break; + + case Orientation: + case RotationVector: + case LinearAccel: + case Gravity: + for (int i = 0; i < numSensors; i++) { + if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { + LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); + return 0; + } + } + break; + } + + // TODO: disabled for GED tablet + // pthread_mutex_lock(&mHALMutex); + int res = update_delay(); + // pthread_mutex_unlock(&mHALMutex); + return res; +} + +int MPLSensor::update_delay() { + VHANDLER_LOG; + + int res = 0; + int64_t got; + + if (mEnabled) { + uint64_t wanted = -1LLU; + uint64_t wanted_ec = -1LLU; + + // Sequence to change sensor's FIFO rate + // 1. enable Power state + // 2. reset master enable + // 3. Update delay + // 4. set master enable + + // TODO: unnecessary for IIO + // ensure power on + // onPower(1); + + // TODO: unnecessary for IIO + // reset master enable + // masterEnable(0); + + /* search the minimum delay requested across all enabled sensors */ + for (int i = 0; i < numSensors; i++) { + if (mEnabled & (1 << i)) { + uint64_t ns = mDelays[i]; + wanted = wanted < ns ? wanted : ns; + } + } + + // same delay for ext compass + wanted_ec = wanted; + + /* mpl rate in us in future maybe different for + gyro vs compass vs accel */ + int rateInus = (int)wanted / 1000LL; + int mplGyroRate = rateInus; + int mplAccelRate = rateInus; + int mplCompassRate = rateInus; + + LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : " + "%llu ns, mpl rate: %d us, (%.2f Hz)", + wanted, rateInus, 1000000000.f / wanted); + + /* 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); + +// TODO: disabled for GED tablet +#ifdef ENABLE_LP_QUAT_FEAT + // Set LP Quaternion sample rate if enabled + if (checkLPQuaternion()) { + // TODO: need to verify this part for LPQ + if (wanted < 5000000LL) { + enableLPQuaternion(0); + } else { + inv_set_quat_sample_rate(rateInus); + // set DMP output rate to FIFO + write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted); + + //DMP running rate must be @ 200Hz + wanted= 5000000LL; + LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus); + } + } +#endif + + /* TODO: Test 200Hz */ + // inv_set_gyro_sample_rate(5000); + LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate); + LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate); + LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate); + + int enabled_sensors = mEnabled; + int tempFd = -1; + if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { + uint64_t tempRate = wanted; + LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); + //nsToHz + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / tempRate, mpu.gyro_fifo_rate, + getTimestamp()); + tempFd = open(mpu.gyro_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / tempRate); + if(res < 0) { + LOGE("HAL:GYRO update delay error"); + } + + //nsToHz (BMA250) + if(USE_THIRD_PARTY_ACCEL == 1) { + LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", + wanted / 1000000L, mpu.accel_fifo_rate, + getTimestamp()); + tempFd = open(mpu.accel_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, wanted / 1000000L); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } + + if (!mCompassSensor->isIntegrated()) { + mCompassSensor->setDelay(ID_M, wanted_ec); + got = mCompassSensor->getDelay(ID_M); + inv_set_compass_sample_rate(got / 1000); + } + + /* + //nsTons - nothing to be done + strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len], + COMPASS_SENSOR_DELAY); + tempFd = open(compass_sensor_sysfs_path, O_RDWR); + LOGV_IF(PROCESS_VERBOSE, "setDelay - open path: %s", compass_sensor_sysfs_path); + wanted = 20000000LLU; + res = write_attribute_sensor(tempFd, wanted); + if(res < 0) { + LOGE("Compass update delay error"); + } + */ + + } else { + + if (GY_ENABLED) { + wanted = mDelays[Gyro]; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp()); + tempFd = open(mpu.gyro_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / wanted); + LOGE_IF(res < 0, "HAL:GYRO update delay error"); + } + + if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */ + if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { + wanted = mDelays[Gyro]; + } else { + wanted = mDelays[Accelerometer]; + } + /* TODO: use function pointers to calculate delay value specific to vendor */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp()); + tempFd = open(mpu.accel_fifo_rate, O_RDWR); + //BMA250 in ms + //res = write_attribute_sensor(tempFd, wanted / 1000000L); + //MPU6050 in hz + res = write_attribute_sensor(tempFd, 1000000000.f/wanted); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } + + /* Invensense compass calibration */ + if (M_ENABLED) { + if (!mCompassSensor->isIntegrated()) { + wanted = mDelays[MagneticField]; + } else { + if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) { + wanted = mDelays[Gyro]; + } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) { + wanted = mDelays[Accelerometer]; + } else { + wanted = mDelays[MagneticField]; + } + } + mCompassSensor->setDelay(ID_M, wanted); + got = mCompassSensor->getDelay(ID_M); + inv_set_compass_sample_rate(got / 1000); + } + + } + + /* + if sensor & THREE_AXIS_GYRO + enable = 1 + if sensor & THREE_AXIS_ACCEL + enable = 1 + if compass_on_secondary + if sensor & THREE_AXIS_COMPASS + enable = 1 + else + enable = 0 + */ + unsigned long sensors = mLocalSensorMask & mMasterSensorMask; + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { +// TODO: unnecessary for IIO +#if 0 + res = masterEnable(1); + if(res < 0) { + return res; + } +#endif + } else { // all sensors idle -> reduce power +// TODO: unnecessary for IIO +#if 0 + res = onPower(0); + if(res < 0) { + return res; + } +#endif + } + } + + return res; +} + +/* use for third party accel */ +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(); + } + + LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived); + + return numEventReceived; +} + +/** + * Should be called after reading at least one of gyro + * compass or accel data. You should only read 1 sample of + * data and call this. + * @returns 0, if successful, error number if not. + */ +int MPLSensor::executeOnData(sensors_event_t* data, int count) +{ + VFUNC_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; + } + } + + // load up virtual sensors + for (int i = 0; i < numSensors; i++) { + int update; + 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; +} + +int MPLSensor::readEvents(sensors_event_t *data, int count) { + + + int lp_quaternion_on, nbyte; + int i, nb, mask = 0, numEventReceived = 0, + sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1: 0) + + (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1: 0); + char *rdata = mIIOBuffer; + + nbyte= (8 * sensors + 8) * 1; + +// TODO: disabled for GED tablet +#ifdef ENABLE_LP_QUAT_FEAT + lp_quaternion_on = checkLPQuaternion(); + if (lp_quaternion_on == 1) { + nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data + } +#endif + + // TODO: disabled for GED tablet + // pthread_mutex_lock(&mMplMutex); + // pthread_mutex_lock(&mHALMutex); + + size_t rsize = read(iio_fd, rdata, nbyte); + if (sensors == 0) { + // read(iio_fd, rdata, nbyte); + read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH); + } +/* + LOGI("get one sample of IIO data with size: %d", rsize); + LOGI("sensors: %d", sensors); + + LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d", + *((short *) (rdata + 0)), *((short *) (rdata + 2)), + *((short *) (rdata + 4))); + LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d", + *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), + *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), + *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); + + LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS & + mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d", + *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), + *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), + *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))); +*/ + + // TODO: need to verify this for LPQ + if (rsize < (nbyte - 8)) { + LOGE("HAL:ERR Full data packet was not read"); + // return -1; + } + +// TODO: disabled for GED tablet +#ifdef ENABLE_LP_QUAT_FEAT + if (lp_quaternion_on == 1) { + for (i=0; i< 4; i++) { + mCachedQuaternionData[i]= *(long*)rdata; + rdata += sizeof(long); + } + } +/* + LOGV("HAL:rdata= %x sensors= %d lp_q_on= %d nbyte= %d rsize= %d", + rdata, sensors, lp_quaternion_on, nbyte, rsize); //tbd +*/ +#endif + + for (i = 0; i < 3; i++) { + if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { + mCachedGyroData[i] = *((short *) (rdata + i * 2)); + } + if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { + mCachedAccelData[i] = *((short *) (rdata + i * 2 + + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); + } + if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) { + mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1))); + } + } + + mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0)); + if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() && + (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) { + mask |= 4; + } + + mSensorTimestamp = *((long long *) (rdata + 8 * sensors)); + if (mCompassSensor->isIntegrated()) { + mCompassTimestamp = mSensorTimestamp; + } + + // send down temperature every 0.5 seconds + if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) { + mTempCurrentTime = mSensorTimestamp; + long long temperature[2]; + if(inv_read_temperature(temperature) == 0) { + LOGV_IF(INPUT_DATA, + "HAL:inv_read_temperature = %lld, timestamp= %lld", + temperature[0], temperature[1]); + inv_build_temp(temperature[0], temperature[1]); + } +#ifdef TESTING + long bias[3], temp, temp_slope[3]; + inv_get_gyro_bias(bias, &temp); + inv_get_gyro_ts(temp_slope); + + LOGI("T: %.3f " + "GB: %+13f %+13f %+13f " + "TS: %+13f %+13f %+13f " + "\n", + (float)temperature[0] / 65536.f, + (float)bias[0] / 65536.f / 16.384f, + (float)bias[1] / 65536.f / 16.384f, + (float)bias[2] / 65536.f / 16.384f, + temp_slope[0] / 65536.f, + temp_slope[1] / 65536.f, + temp_slope[2] / 65536.f); +#endif + } + + if (mask & 1) { + mPendingMask |= 1 << Gyro; + if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { + inv_build_gyro(mCachedGyroData, mSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld", + mCachedGyroData[0], mCachedGyroData[1], + mCachedGyroData[2], mSensorTimestamp); + } + } + + if (mask & 2) { + mPendingMask |= 1 << Accelerometer; + if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { + inv_build_accel(mCachedAccelData, 0, mSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld", + mCachedAccelData[0], mCachedAccelData[1], + mCachedAccelData[2], mSensorTimestamp); + } + } + + if ((mask & 4) && mCompassSensor->isIntegrated()) { + 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:inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], + mCachedCompassData[2], mCompassTimestamp); + } + } + +// TODO: disabled for GED tablet +#ifdef ENABLE_LP_QUAT_FEAT + if (lp_quaternion_on == 1) { + inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp); + LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld - %d", + mCachedQuaternionData[0], mCachedQuaternionData[1], + mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp); + } +#endif + + // pthread_mutex_unlock(&mMplMutex); + // pthread_mutex_unlock(&mHALMutex); + + return numEventReceived; +} + +/* use for both MPUxxxx and third party compass */ +int MPLSensor::readCompassEvents(sensors_event_t *data, int count) +{ + VHANDLER_LOG; + + if (count < 1) + return -EINVAL; + + int numEventReceived = 0; + int done = 0; + int nb; + + // TODO: disabled for GED tablet + // TODO: for AMI306 + // pthread_mutex_lock(&mMplMutex); + // pthread_mutex_lock(&mHALMutex); + + done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); + 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:inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], + mCachedCompassData[2], mCompassTimestamp); + } + } + + // pthread_mutex_unlock(&mMplMutex); + // pthread_mutex_unlock(&mHALMutex); + + return numEventReceived; +} + +int MPLSensor::getFd() const +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd); + return iio_fd; +} + +int MPLSensor::getAccelFd() const +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd); + return accel_fd; +} + +int MPLSensor::getCompassFd() const +{ + VFUNC_LOG; + int fd = mCompassSensor->getFd(); + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd); + return fd; +} + +int MPLSensor::getPollTime() +{ + VHANDLER_LOG; + return mPollTime; +} + +bool MPLSensor::hasPendingEvents() const +{ + VHANDLER_LOG; + // if we are using the polling workaround, force the main + // loop to check for data every time + return (mPollTime != -1); +} + +/* TODO: support resume suspend when we gain more info about them*/ +void MPLSensor::sleepEvent() +{ + VFUNC_LOG; +} + +void MPLSensor::wakeEvent() +{ + VFUNC_LOG; +} + +int MPLSensor::inv_float_to_q16(float *fdata, long *ldata) +{ + VHANDLER_LOG; + + 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 MPLSensor::inv_long_to_q16(long *fdata, long *ldata) +{ + VHANDLER_LOG; + + 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 MPLSensor::inv_float_to_round(float *fdata, long *ldata) +{ + VHANDLER_LOG; + + if (!fdata || !ldata) + return -1; + ldata[0] = (long)fdata[0]; + ldata[1] = (long)fdata[1]; + ldata[2] = (long)fdata[2]; + return 0; +} + +int MPLSensor::inv_float_to_round2(float *fdata, short *ldata) +{ + VHANDLER_LOG; + + if (!fdata || !ldata) + return -1; + ldata[0] = (short)fdata[0]; + ldata[1] = (short)fdata[1]; + ldata[2] = (short)fdata[2]; + return 0; +} + +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, + "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)(7 * sizeof(sensor_t))) { + LOGE("HAL:sensor list too small, not populating."); + return 0; + } + + /* fill in the base values */ + memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); + + /* 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]); + + if(1) { + numsensors = 7; + /* all sensors will be added to the list + fill in orientation values */ + fillOrientation(list); + /* fill in rotation vector values */ + fillRV(list); + /* fill in gravity values */ + fillGravity(list); + /* fill in Linear accel values */ + fillLinearAccel(list); + } 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; + + // TODO: for GED tablet + // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY; + list[Accelerometer].minDelay = 5000; + + 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, "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 bma250 and might be wrong.", + accel); + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + list[Accelerometer].minDelay = ACCEL_BMA250_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; + + // TODO: for GED tablet + // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY; + list[Gyro].minDelay = 5000; + } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) { + list[Gyro].maxRange = GYRO_MPU9150_RANGE; + list[Gyro].resolution = GYRO_MPU9150_RESOLUTION; + list[Gyro].power = GYRO_MPU9150_POWER; + list[Gyro].minDelay = GYRO_MPU9150_MINDELAY; + } else { + LOGE("HAL:unknown gyro id -- gyro params will be wrong."); + LOGE("HAL:default to use mpu3050 params"); + list[Gyro].maxRange = GYRO_MPU3050_RANGE; + list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; + list[Gyro].power = GYRO_MPU3050_POWER; + list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; + } + return; +} + +/* fillRV depends on values of 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; + + // TODO: for GED tablet + // list[RotationVector].minDelay = 10000; + list[RotationVector].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; + + // TODO: for GED tablet + // list[Orientation].minDelay = 10000; + 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; + + // TODO: for GED tablet + // list[Gravity].minDelay = 10000; + 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; + + // TODO: for GED tablet + // list[LinearAccel].minDelay = 10000; + list[LinearAccel].minDelay = 5000; + + return; +} + +int MPLSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + unsigned char i = 0; + char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2]; + char *sptr; + char **dptr; + int num; + + sysfs_names_ptr = + (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + sptr = sysfs_names_ptr; + if (sptr != NULL) { + dptr = (char**)&mpu; + do { + *dptr++ = sptr; + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < MAX_SYSFS_ATTRB); + } else { + LOGE("HAL:couldn't alloc mem for sysfs paths"); + return -1; + } + + // get proper (in absolute/relative) IIO path & build MPU's sysfs paths + // inv_get_sysfs_abs_path(sysfs_path); + inv_get_sysfs_path(sysfs_path); + inv_get_iio_trigger_path(iio_trigger_path); + + sprintf(mpu.key, "%s%s", sysfs_path, "/key"); + sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); + sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length"); + 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.trigger_name, "%s%s", iio_trigger_path, "/name"); + sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger"); + + 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_output_rate,"%s%s", sysfs_path, "/dmp_output_rate"); + sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on"); + + // TODO: for self test + 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_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en"); + sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en"); + sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en"); + +#ifdef THIRD_PARTY_ACCEL //BMA250 + /* same as 'mpu.accel_enable' */ + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/enable"); + sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate"); + sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA"); + sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA"); + sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/NA"); +#else + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); + sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); + sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); + sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en"); + sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en"); + sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en"); + + // TODO: for bias settings + sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias"); + + sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); +#endif + + sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on"); + sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en"); + sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en"); + sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en"); + sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en"); + +#if 0 + // test print sysfs paths + dptr = (char**)&mpu; + for (i = 0; i < MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} diff --git a/libsensors_iio/MPLSensor.h b/libsensors_iio/MPLSensor.h new file mode 100644 index 0000000..4c38c57 --- /dev/null +++ b/libsensors_iio/MPLSensor.h @@ -0,0 +1,282 @@ +/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef ANDROID_MPL_SENSOR_H
+#define ANDROID_MPL_SENSOR_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <poll.h>
+#include <utils/Vector.h>
+#include <utils/KeyedVector.h>
+#include "sensors.h"
+#include "SensorBase.h"
+#include "InputEventReader.h"
+
+// TODO: change to wanted vendor
+#ifdef INVENSENSE_COMPASS_CAL
+
+#ifdef COMPASS_AMI306
+#warning "unified HAL for AMI306"
+#include "CompassSensor.IIO.AMI.h"
+#else
+#warning "unified HAL for MPU9150"
+#include "CompassSensor.IIO.9150.h"
+#endif
+
+#else
+#warning "unified HAL for AKM"
+#include "CompassSensor.AKM.h"
+#endif
+
+/*****************************************************************************/
+/* Sensors Enable/Disable Mask
+ *****************************************************************************/
+#define MAX_CHIP_ID_LEN (20)
+#define IIO_BUFFER_LENGTH (100 * 2)
+
+#define INV_THREE_AXIS_GYRO (0x000F)
+#define INV_THREE_AXIS_ACCEL (0x0070)
+#define INV_THREE_AXIS_COMPASS (0x0380)
+#define INV_ALL_SENSORS (0x7FFF)
+
+#ifdef INVENSENSE_COMPASS_CAL
+#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
+ | INV_THREE_AXIS_COMPASS \
+ | INV_THREE_AXIS_GYRO)
+#else
+// TODO: ID_M = 2 even for 3rd-party solution
+#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
+ | INV_THREE_AXIS_COMPASS \
+ | INV_THREE_AXIS_GYRO)
+#endif
+
+// bit mask of current active features (mFeatureActiveMask)
+#define INV_COMPASS_CAL 0x01
+#define INV_COMPASS_FIT 0x02
+#define INV_DMP_QUATERNION 0x04
+
+/*****************************************************************************/
+/** MPLSensor implementation which fits into the HAL example for crespo provided
+ * by Google.
+ * WARNING: there may only be one instance of MPLSensor, ever.
+ */
+
+class MPLSensor: public SensorBase
+{
+ typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
+
+public:
+ MPLSensor(CompassSensor *);
+ virtual ~MPLSensor();
+
+ enum
+ {
+ Gyro = 0,
+ Accelerometer,
+ MagneticField,
+ Orientation,
+ RotationVector,
+ LinearAccel,
+ Gravity,
+ numSensors
+ };
+
+ virtual int setDelay(int32_t handle, int64_t ns);
+ virtual int enable(int32_t handle, int enabled);
+ int32_t getEnableMask() { return mEnabled; }
+
+ virtual int readEvents(sensors_event_t *data, int count);
+ virtual int getFd() const;
+ virtual int getAccelFd() const;
+ virtual int getCompassFd() const;
+ virtual int getPollTime();
+ virtual bool hasPendingEvents() const;
+ virtual void sleepEvent();
+ virtual void wakeEvent();
+ int populateSensorList(struct sensor_t *list, int len);
+ void cbProcData();
+
+ //static pointer to the object that will handle callbacks
+ static MPLSensor* gMPLSensor;
+
+ //AKM HAL Integration
+ //void set_compass(long ready, long x, long y, long z, long accuracy);
+ int executeOnData(sensors_event_t* data, int count);
+ int readAccelEvents(sensors_event_t* data, int count);
+ int readCompassEvents(sensors_event_t* data, int count);
+
+protected:
+ CompassSensor *mCompassSensor;
+
+ int gyroHandler(sensors_event_t *data);
+ int accelHandler(sensors_event_t *data);
+ int compassHandler(sensors_event_t *data);
+ int rvHandler(sensors_event_t *data);
+ int laHandler(sensors_event_t *data);
+ int gravHandler(sensors_event_t *data);
+ int orienHandler(sensors_event_t *data);
+ void calcOrientationSensor(float *Rx, float *Val);
+ virtual int update_delay();
+
+ void inv_set_device_properties();
+ int inv_constructor_init();
+ int inv_constructor_default_enable();
+ int setGyroInitialState();
+ int setAccelInitialState();
+ int masterEnable(int en);
+ int onPower(int en);
+ int enableLPQuaternion(int);
+ int enableQuaternionData(int);
+ int onDMP(int);
+ int enableGyro(int en);
+ int enableAccel(int en);
+ int enableCompass(int en);
+ void computeLocalSensorMask(int enabled_sensors);
+ int enableSensors(unsigned long sensors, int en, uint32_t changed);
+ int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
+ int update_delay_sysfs_sensor(int fd, uint64_t ns);
+ int inv_float_to_q16(float *fdata, long *ldata);
+ int inv_long_to_q16(long *fdata, long *ldata);
+ int inv_float_to_round(float *fdata, long *ldata);
+ int inv_float_to_round2(float *fdata, short *sdata);
+ int inv_read_temperature(long long *data);
+ int inv_read_dmp_state(int fd);
+ int inv_read_sensor_bias(int fd, long *data);
+ void inv_get_sensors_orientation(void);
+ int inv_init_sysfs_attributes(void);
+ void setCompassDelay(int64_t ns);
+ void enable_iio_sysfs(void);
+ int enableTap(int);
+ int enableFlick(int);
+ int enablePedometer(int);
+ int checkLPQuaternion();
+
+ int mNewData; // flag indicating that the MPL calculated new output values
+ int mDmpStarted;
+ long mMasterSensorMask;
+ long mLocalSensorMask;
+ int mPollTime;
+ bool mHaveGoodMpuCal; // flag indicating that the cal file can be written
+ int mGyroAccuracy; // value indicating the quality of the gyro calibr.
+ int mAccelAccuracy; // value indicating the quality of the accel calibr.
+ struct pollfd mPollFds[5];
+ int mSampleCount;
+ pthread_mutex_t mMplMutex;
+ pthread_mutex_t mHALMutex;
+
+ char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
+
+ int iio_fd;
+ int accel_fd;
+ int mpufifo_fd;
+ int gyro_temperature_fd;
+
+ uint32_t mEnabled;
+ uint32_t mOldEnabledMask;
+ sensors_event_t mPendingEvents[numSensors];
+ uint64_t mDelays[numSensors];
+ hfunc_t mHandlers[numSensors];
+ short mCachedGyroData[3];
+ long mCachedAccelData[3];
+ long mCachedCompassData[3];
+ long mCachedQuaternionData[4];
+ android::KeyedVector<int, int> mIrqFds;
+
+ InputEventCircularReader mAccelInputReader;
+ InputEventCircularReader mGyroInputReader;
+
+ bool mFirstRead;
+ short mTempScale;
+ short mTempOffset;
+ int64_t mTempCurrentTime;
+ int mAccelScale;
+
+ uint32_t mPendingMask;
+ unsigned long mSensorMask;
+
+ char chip_ID[MAX_CHIP_ID_LEN];
+
+ signed char mGyroOrientation[9];
+ signed char mAccelOrientation[9];
+
+ int64_t mSensorTimestamp;
+ int64_t mCompassTimestamp;
+
+ struct sysfs_attrbs {
+ char *chip_enable;
+ char *power_state;
+ char *dmp_firmware;
+ char *firmware_loaded;
+ char *dmp_on;
+ char *dmp_int_on;
+ char *tap_on;
+ char *key;
+ char *self_test;
+ char *temperature;
+ char *dmp_output_rate;
+
+ char *gyro_enable;
+ char *gyro_fifo_rate;
+ char *gyro_orient;
+ char *gyro_x_fifo_enable;
+ char *gyro_y_fifo_enable;
+ char *gyro_z_fifo_enable;
+
+ char *accel_enable;
+ char *accel_fifo_rate;
+ char *accel_fsr;
+ char *accel_bias;
+ char *accel_orient;
+ char *accel_x_fifo_enable;
+ char *accel_y_fifo_enable;
+ char *accel_z_fifo_enable;
+
+ char *quaternion_on;
+ char *in_quat_r_en;
+ char *in_quat_x_en;
+ char *in_quat_y_en;
+ char *in_quat_z_en;
+
+ char *in_timestamp_en;
+ char *trigger_name;
+ char *current_trigger;
+ char *buffer_length;
+ } mpu;
+
+ char *sysfs_names_ptr;
+ int mFeatureActiveMask;
+
+private:
+ /* added for dynamic get sensor list */
+ void fillAccel(const char* accel, struct sensor_t *list);
+ void fillGyro(const char* gyro, struct sensor_t *list);
+ void fillRV(struct sensor_t *list);
+ void fillOrientation(struct sensor_t *list);
+ void fillGravity(struct sensor_t *list);
+ void fillLinearAccel(struct sensor_t *list);
+ void storeCalibration();
+ void loadDMP();
+};
+
+extern "C" {
+ void setCallbackObject(MPLSensor*);
+ MPLSensor *getCallbackObject();
+}
+
+#endif // ANDROID_MPL_SENSOR_H
diff --git a/libsensors_iio/MPLSupport.cpp b/libsensors_iio/MPLSupport.cpp new file mode 100644 index 0000000..a961d78 --- /dev/null +++ b/libsensors_iio/MPLSupport.cpp @@ -0,0 +1,144 @@ +#include <MPLSupport.h> +#include <string.h> +#include <stdio.h> +#include "log.h" +#include "SensorBase.h" +#include <fcntl.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 = -1; + int err = 0; + + if (fd >= 0) { + char buf[2]; + if (en) { + buf[0] = '1'; + nb = write(fd, buf, 1); + } else { + buf[0] = '0'; + nb = write(fd, buf, 1); + } + buf[1] = '\0'; + + if (nb <= 0) { + err = errno; + LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)", + buf[0], nb, strerror(err), err); + } + close(fd); + } else { + LOGV_IF(EXTRA_VERBOSE, "HAL:enable_sysfs_sensor - fd<0"); + } + + 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; +} + +int read_sysfs_int(char *filename, int *var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "r"); + if (sysfsfp!=NULL) { + fscanf(sysfsfp, "%d\n", var); + fclose(sysfsfp); + } else { + LOGE("HAL:ERR open file to read"); + res= -1; + } + return res; +} + +int write_sysfs_int(char *filename, int var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "w"); + if (sysfsfp!=NULL) { + fprintf(sysfsfp, "%d\n", var); + fclose(sysfsfp); + } else { + LOGE("HAL:ERR open file to write"); + res= -1; + } + return res; +} diff --git a/libsensors_iio/MPLSupport.h b/libsensors_iio/MPLSupport.h new file mode 100644 index 0000000..73604ba --- /dev/null +++ b/libsensors_iio/MPLSupport.h @@ -0,0 +1,29 @@ +/* +* 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 read_sysfs_int(char*, int*); + int write_sysfs_int(char*, int); + +#endif // ANDROID_MPL_SUPPORT_H diff --git a/libsensors_iio/SensorBase.cpp b/libsensors_iio/SensorBase.cpp new file mode 100644 index 0000000..fd0e2ca --- /dev/null +++ b/libsensors_iio/SensorBase.cpp @@ -0,0 +1,143 @@ +/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/select.h>
+#include <cutils/log.h>
+#include <linux/input.h>
+
+#include "SensorBase.h"
+#include "local_log_def.h"
+
+/*****************************************************************************/
+
+SensorBase::SensorBase(const char* dev_name,
+ const char* data_name) : dev_name(dev_name),
+ data_name(data_name),
+ dev_fd(-1),
+ data_fd(-1)
+{
+ if (data_name) {
+ data_fd = openInput(data_name);
+ }
+}
+
+SensorBase::~SensorBase()
+{
+ if (data_fd >= 0) {
+ close(data_fd);
+ }
+ if (dev_fd >= 0) {
+ close(dev_fd);
+ }
+}
+
+int SensorBase::open_device()
+{
+ if (dev_fd<0 && dev_name) {
+ dev_fd = open(dev_name, O_RDONLY);
+ LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
+ }
+ return 0;
+}
+
+int SensorBase::close_device()
+{
+ if (dev_fd >= 0) {
+ close(dev_fd);
+ dev_fd = -1;
+ }
+ return 0;
+}
+
+int SensorBase::getFd() const
+{
+ if (!data_name) {
+ return dev_fd;
+ }
+ return data_fd;
+}
+
+int SensorBase::setDelay(int32_t handle, int64_t ns)
+{
+ return 0;
+}
+
+bool SensorBase::hasPendingEvents() const
+{
+ return false;
+}
+
+int64_t SensorBase::getTimestamp()
+{
+ struct timespec t;
+ t.tv_sec = t.tv_nsec = 0;
+ clock_gettime(CLOCK_MONOTONIC, &t);
+ return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+}
+
+int SensorBase::openInput(const char *inputName)
+{
+ int fd = -1;
+ const char *dirname = "/dev/input";
+ char devname[PATH_MAX];
+ char *filename;
+ DIR *dir;
+ struct dirent *de;
+ dir = opendir(dirname);
+ if(dir == NULL)
+ return -1;
+ strcpy(devname, dirname);
+ filename = devname + strlen(devname);
+ *filename++ = '/';
+ while((de = readdir(dir))) {
+ if(de->d_name[0] == '.' &&
+ (de->d_name[1] == '\0' ||
+ (de->d_name[1] == '.' && de->d_name[2] == '\0')))
+ continue;
+ strcpy(filename, de->d_name);
+ fd = open(devname, O_RDONLY);
+ LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
+ LOGI("path open %s", devname);
+ if (fd >= 0) {
+ char name[80];
+ if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
+ name[0] = '\0';
+ }
+ LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
+ if (!strcmp(name, inputName)) {
+ strcpy(input_name, filename);
+ break;
+ } else {
+ close(fd);
+ fd = -1;
+ }
+ }
+ }
+ closedir(dir);
+ LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
+ return fd;
+}
+
+int SensorBase::enable(int32_t handle, int enabled)
+{
+ return 0;
+}
diff --git a/libsensors_iio/SensorBase.h b/libsensors_iio/SensorBase.h new file mode 100644 index 0000000..d9abe92 --- /dev/null +++ b/libsensors_iio/SensorBase.h @@ -0,0 +1,64 @@ +/* +* 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> + +#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember)) + +#define MAX_SYSFS_NAME_LEN (100) + +/*****************************************************************************/ + +struct sensors_event_t; + +class SensorBase { +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; + 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); +}; + +/*****************************************************************************/ + +#endif // ANDROID_SENSOR_BASE_H diff --git a/libsensors_iio/libmllite.so b/libsensors_iio/libmllite.so Binary files differnew file mode 100644 index 0000000..ed13b13 --- /dev/null +++ b/libsensors_iio/libmllite.so diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so Binary files differnew file mode 100644 index 0000000..e2ab461 --- /dev/null +++ b/libsensors_iio/libmplmpu.so diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h new file mode 100644 index 0000000..b08fdf1 --- /dev/null +++ b/libsensors_iio/local_log_def.h @@ -0,0 +1,46 @@ +#ifndef LOCAL_LOG_DEF_H
+#define LOCAL_LOG_DEF_H
+
+// -- workaround for different LOG definition in JellyBean --
+#define LOGV ALOGV
+#define LOGV_IF ALOGV_IF
+#define LOGD ALOGD
+#define LOGD_IF ALOGD_IF
+#define LOGI ALOGI
+#define LOGI_IF ALOGI_IF
+#define LOGW ALOGW
+#define LOGW_IF ALOGW_IF
+#define LOGE ALOGE
+#define LOGE_IF ALOGE_IF
+#define IF_LOGV IF_ALOGV
+#define IF_LOGD IF_ALOGD
+#define IF_LOGI IF_ALOGI
+#define IF_LOGW IF_ALOGW
+#define IF_LOGE IF_ALOGE
+#define LOG_ASSERT ALOG_ASSERT
+#define LOG ALOG
+#define IF_LOG IF_ALOG
+// -- Workaround End --
+
+/* Log enablers, each of these independent */
+
+#define PROCESS_VERBOSE (0) /* process log messages */
+#define EXTRA_VERBOSE (0) /* verbose log messages */
+#define SYSFS_VERBOSE (0) /* log sysfs interactions as cat/echo for repro
+ purpose on a shell */
+#define FUNC_ENTRY (0) /* log entry in all one-time functions */
+
+/* Note that enabling this logs may affect performance */
+#define HANDLER_ENTRY (0) /* log entry in all handler functions */
+#define ENG_VERBOSE (0) /* log some a lot more info about the internals */
+#define INPUT_DATA (0) /* log the data input from the events */
+#define HANDLER_DATA (0) /* log the data fetched from the handlers */
+
+#define FUNC_LOG \
+ LOGV("%s", __PRETTY_FUNCTION__)
+#define VFUNC_LOG \
+ LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)
+#define VHANDLER_LOG \
+ LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
+
+#endif
diff --git a/libsensors_iio/sensor_params.h b/libsensors_iio/sensor_params.h new file mode 100644 index 0000000..88d5ba0 --- /dev/null +++ b/libsensors_iio/sensor_params.h @@ -0,0 +1,162 @@ +/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#ifndef INV_SENSOR_PARAMS_H
+#define INV_SENSOR_PARAMS_H
+
+/* Physical parameters of the sensors supported by Invensense MPL */
+#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV)
+#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA)
+#define SENSORS_GRAVITY_HANDLE (ID_GR)
+#define SENSORS_GYROSCOPE_HANDLE (ID_GY)
+#define SENSORS_ACCELERATION_HANDLE (ID_A)
+#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M)
+#define SENSORS_ORIENTATION_HANDLE (ID_O)
+
+/******************************************/
+//MPU9150 INV_COMPASS
+#define COMPASS_MPU9150_RANGE (9830.f)
+#define COMPASS_MPU9150_RESOLUTION (0.285f)
+#define COMPASS_MPU9150_POWER (10.f)
+#define COMPASS_MPU9150_MINDELAY (10000)
+//COMPASS_ID_AKM
+#define COMPASS_AKM8975_RANGE (9830.f)
+#define COMPASS_AKM8975_RESOLUTION (0.285f)
+#define COMPASS_AKM8975_POWER (10.f)
+#define COMPASS_AKM8975_MINDELAY (10000)
+//COMPASS_ID_AMI30X
+#define COMPASS_AMI30X_RANGE (5461.f)
+#define COMPASS_AMI30X_RESOLUTION (0.9f)
+#define COMPASS_AMI30X_POWER (0.15f)
+//COMPASS_ID_AMI306
+#define COMPASS_AMI306_RANGE (5461.f)
+#define COMPASS_AMI306_RESOLUTION (0.9f)
+#define COMPASS_AMI306_POWER (0.15f)
+#define COMPASS_AMI306_MINDELAY (10000)
+//COMPASS_ID_YAS529
+#define COMPASS_YAS529_RANGE (19660.f)
+#define COMPASS_YAS529_RESOLUTION (0.012f)
+#define COMPASS_YAS529_POWER (4.f)
+//COMPASS_ID_YAS530
+#define COMPASS_YAS530_RANGE (8001.f)
+#define COMPASS_YAS530_RESOLUTION (0.012f)
+#define COMPASS_YAS530_POWER (4.f)
+#define COMPASS_YAS530_MINDELAY (10000)
+//COMPASS_ID_HMC5883
+#define COMPASS_HMC5883_RANGE (10673.f)
+#define COMPASS_HMC5883_RESOLUTION (10.f)
+#define COMPASS_HMC5883_POWER (0.24f)
+//COMPASS_ID_LSM303DLH
+#define COMPASS_LSM303DLH_RANGE (10240.f)
+#define COMPASS_LSM303DLH_RESOLUTION (1.f)
+#define COMPASS_LSM303DLH_POWER (1.f)
+//COMPASS_ID_LSM303DLM
+#define COMPASS_LSM303DLM_RANGE (10240.f)
+#define COMPASS_LSM303DLM_RESOLUTION (1.f)
+#define COMPASS_LSM303DLM_POWER (1.f)
+//COMPASS_ID_MMC314X
+#define COMPASS_MMC314X_RANGE (400.f)
+#define COMPASS_MMC314X_RESOLUTION (2.f)
+#define COMPASS_MMC314X_POWER (0.55f)
+//COMPASS_ID_HSCDTD002B
+#define COMPASS_HSCDTD002B_RANGE (9830.f)
+#define COMPASS_HSCDTD002B_RESOLUTION (1.f)
+#define COMPASS_HSCDTD002B_POWER (1.f)
+//COMPASS_ID_HSCDTD004A
+#define COMPASS_HSCDTD004A_RANGE (9830.f)
+#define COMPASS_HSCDTD004A_RESOLUTION (1.f)
+#define COMPASS_HSCDTD004A_POWER (1.f)
+/*******************************************/
+//ACCEL_ID_MPU9150
+#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU9150_POWER (0.f)
+#define ACCEL_MPU9150_MINDELAY (1000)
+//ACCEL_ID_LIS331
+#define ACCEL_LIS331_RANGE (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS331_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS331_POWER (1.f)
+//ACCEL_ID_LSM303DLX
+#define ACCEL_LSM303DLX_RANGE (2.48f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_LSM303DLX_POWER (1.f)
+//ACCEL_ID_LIS3DH
+#define ACCEL_LIS3DH_RANGE (2.48f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_LIS3DH_POWER (1.f)
+//ACCEL_ID_KXSD9
+#define ACCEL_KXSD9_RANGE (2.5006f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_KXSD9_POWER (1.f)
+//ACCEL_ID_KXTF9
+#define ACCEL_KXTF9_RANGE (1.f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_RESOLUTION (0.033f * GRAVITY_EARTH)
+#define ACCEL_KXTF9_POWER (0.35f)
+//ACCEL_ID_BMA150
+#define ACCEL_BMA150_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA150_RESOLUTION (0.004f * GRAVITY_EARTH)
+#define ACCEL_BMA150_POWER (0.2f)
+//ACCEL_ID_BMA222
+#define ACCEL_BMA222_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA222_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_BMA222_POWER (0.1f)
+//ACCEL_ID_BMA250
+#define ACCEL_BMA250_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_BMA250_RESOLUTION (0.00391f * GRAVITY_EARTH)
+#define ACCEL_BMA250_POWER (0.139f)
+#define ACCEL_BMA250_MINDELAY (1000)
+//ACCEL_ID_ADXL34X
+#define ACCEL_ADXL34X_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_ADXL34X_POWER (1.f)
+//ACCEL_ID_MMA8450
+#define ACCEL_MMA8450_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA8450_POWER (1.0f)
+//ACCEL_ID_MMA845X
+#define ACCEL_MMA845X_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_RESOLUTION (0.001f * GRAVITY_EARTH)
+#define ACCEL_MMA845X_POWER (1.f)
+//ACCEL_ID_MPU6050
+#define ACCEL_MPU6050_RANGE (2.f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_RESOLUTION (0.004f * GRAVITY_EARTH)
+#define ACCEL_MPU6050_POWER (0.f)
+#define ACCEL_MPU6050_MINDELAY (1000)
+/******************************************/
+//GYRO MPU3050
+#define RAD_P_DEG (3.14159f / 180.f)
+#define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_MPU3050_RESOLUTION (32.8f * RAD_P_DEG)
+#define GYRO_MPU3050_POWER (6.1f)
+#define GYRO_MPU3050_MINDELAY (1000)
+//GYRO MPU6050
+#define GYRO_MPU6050_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_MPU6050_RESOLUTION (16.4f * RAD_P_DEG)
+#define GYRO_MPU6050_POWER (5.5f)
+#define GYRO_MPU6050_MINDELAY (1000)
+//GYRO MPU9150
+#define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_MPU9150_RESOLUTION (16.4f * RAD_P_DEG)
+#define GYRO_MPU9150_POWER (5.5f)
+#define GYRO_MPU9150_MINDELAY (1000)
+//GYRO ITG3500
+#define GYRO_ITG3500_RANGE (2000.f * RAD_P_DEG)
+#define GYRO_ITG3500_RESOLUTION (16.4f * RAD_P_DEG)
+#define GYRO_ITG3500_POWER (5.5f)
+#define GYRO_ITG3500_MINDELAY (1000)
+
+#endif /* INV_SENSOR_PARAMS_H */
+
diff --git a/libsensors_iio/sensors.h b/libsensors_iio/sensors.h new file mode 100644 index 0000000..0556c10 --- /dev/null +++ b/libsensors_iio/sensors.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_SENSORS_H
+#define ANDROID_SENSORS_H
+
+#include <stdint.h>
+#include <errno.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+
+#include <linux/input.h>
+
+#include <hardware/hardware.h>
+#include <hardware/sensors.h>
+
+__BEGIN_DECLS
+
+/*****************************************************************************/
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+#endif
+
+#define ID_MPL_BASE (0)
+#define ID_GY (ID_MPL_BASE)
+#define ID_A (ID_GY + 1)
+#define ID_M (ID_A + 1)
+#define ID_O (ID_M + 1)
+#define ID_RV (ID_O + 1)
+#define ID_LA (ID_RV + 1)
+#define ID_GR (ID_LA + 1)
+
+/*#define ID_MPL_BASE (0)
+#define ID_RV (ID_MPL_BASE)
+#define ID_LA (ID_RV + 1)
+#define ID_GR (ID_LA + 1)
+#define ID_GY (ID_GR + 1)
+#define ID_A (ID_GY + 1)
+#define ID_M (ID_A + 1)
+#define ID_O (ID_M + 1)
+*/
+
+/*****************************************************************************/
+
+/*
+ * The SENSORS Module
+ */
+
+/* ITG3500 */
+#define EVENT_TYPE_GYRO_X REL_X
+#define EVENT_TYPE_GYRO_Y REL_Y
+#define EVENT_TYPE_GYRO_Z REL_Z
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_IACCEL_X REL_RX
+#define EVENT_TYPE_IACCEL_Y REL_RY
+#define EVENT_TYPE_IACCEL_Z REL_RZ
+/* MPU6050 MPU9150 */
+#define EVENT_TYPE_ICOMPASS_X REL_X
+#define EVENT_TYPE_ICOMPASS_Y REL_Y
+#define EVENT_TYPE_ICOMPASS_Z REL_Z
+/* MPUxxxx */
+#define EVENT_TYPE_TIMESTAMP_HI REL_MISC
+#define EVENT_TYPE_TIMESTAMP_LO REL_WHEEL
+
+/* Accel BMA250 */
+#define EVENT_TYPE_ACCEL_X ABS_X
+#define EVENT_TYPE_ACCEL_Y ABS_Y
+#define EVENT_TYPE_ACCEL_Z ABS_Z
+#define LSG (1000.0f)
+
+// conversion of acceleration data to SI units (m/s^2)
+#define RANGE_A (4*GRAVITY_EARTH)
+#define RESOLUTION_A (GRAVITY_EARTH / LSG)
+#define CONVERT_A (GRAVITY_EARTH / LSG)
+#define CONVERT_A_X (CONVERT_A)
+#define CONVERT_A_Y (CONVERT_A)
+#define CONVERT_A_Z (CONVERT_A)
+
+/* Compass AKM8975 */
+#define EVENT_TYPE_MAGV_X ABS_RX
+#define EVENT_TYPE_MAGV_Y ABS_RY
+#define EVENT_TYPE_MAGV_Z ABS_RZ
+#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER
+
+// conversion of magnetic data to uT units
+#define CONVERT_M (0.06f)
+
+__END_DECLS
+
+#endif // ANDROID_SENSORS_H
diff --git a/libsensors_iio/sensors_mpl.cpp b/libsensors_iio/sensors_mpl.cpp new file mode 100644 index 0000000..990c5f5 --- /dev/null +++ b/libsensors_iio/sensors_mpl.cpp @@ -0,0 +1,236 @@ +/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
+
+#include <hardware/sensors.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <dirent.h>
+#include <math.h>
+#include <poll.h>
+#include <pthread.h>
+#include <stdlib.h>
+
+#include <linux/input.h>
+
+#include <utils/Atomic.h>
+#include <utils/Log.h>
+
+#include "sensors.h"
+#include "MPLSensor.h"
+#include "local_log_def.h"
+
+/*****************************************************************************/
+/* The SENSORS Module */
+#define LOCAL_SENSORS (7)
+
+static struct sensor_t sSensorList[7];
+static int numSensors = LOCAL_SENSORS;
+
+static int open_sensors(const struct hw_module_t* module, const char* id,
+ struct hw_device_t** device);
+
+static int sensors__get_sensors_list(struct sensors_module_t* module,
+ struct sensor_t const** list)
+{
+ *list = sSensorList;
+ return numSensors;
+}
+
+static struct hw_module_methods_t sensors_module_methods = {
+ open: open_sensors
+};
+
+struct sensors_module_t HAL_MODULE_INFO_SYM = {
+ common: {
+ tag: HARDWARE_MODULE_TAG,
+ version_major: 1,
+ version_minor: 0,
+ id: SENSORS_HARDWARE_MODULE_ID,
+ name: "Invensense module",
+ author: "Invensense Inc.",
+ methods: &sensors_module_methods,
+ },
+ get_sensors_list: sensors__get_sensors_list,
+};
+
+struct sensors_poll_context_t {
+ struct sensors_poll_device_t device; // must be first
+
+ sensors_poll_context_t();
+ ~sensors_poll_context_t();
+ int activate(int handle, int enabled);
+ int setDelay(int handle, int64_t ns);
+ int pollEvents(sensors_event_t* data, int count);
+
+private:
+ enum {
+ mpl = 0,
+ compass,
+ numSensorDrivers, // wake pipe goes here
+ numFds,
+ };
+
+ struct pollfd mPollFds[numSensorDrivers];
+ SensorBase *mSensor;
+ CompassSensor *mCompassSensor;
+};
+
+/******************************************************************************/
+
+sensors_poll_context_t::sensors_poll_context_t() {
+ VFUNC_LOG;
+
+ CompassSensor *mCompassSensor = new CompassSensor();
+ MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
+
+ // setup the callback object for handing mpl callbacks
+ setCallbackObject(mplSensor);
+
+ // populate the sensor list
+ numSensors =
+ mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
+
+ mSensor = mplSensor;
+ mPollFds[mpl].fd = mSensor->getFd();
+ mPollFds[mpl].events = POLLIN;
+ mPollFds[mpl].revents = 0;
+
+ mPollFds[compass].fd = mCompassSensor->getFd();
+ mPollFds[compass].events = POLLIN;
+ mPollFds[compass].revents = 0;
+}
+
+sensors_poll_context_t::~sensors_poll_context_t() {
+ FUNC_LOG;
+ delete mSensor;
+ delete mCompassSensor;
+}
+
+int sensors_poll_context_t::activate(int handle, int enabled) {
+ FUNC_LOG;
+ return mSensor->enable(handle, enabled);
+}
+
+int sensors_poll_context_t::setDelay(int handle, int64_t ns)
+{
+ FUNC_LOG;
+ return mSensor->setDelay(handle, ns);
+}
+
+int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
+{
+ VHANDLER_LOG;
+
+ int nbEvents = 0;
+ int nb, polltime = -1;
+
+ // look for new events
+ nb = poll(mPollFds, numSensorDrivers, polltime);
+
+ if (nb > 0) {
+ for (int i = 0; count && i < numSensorDrivers; i++) {
+ if (mPollFds[i].revents & POLLIN) {
+ nb = 0;
+ if (i == mpl) {
+ nb = mSensor->readEvents(data, count);
+ }
+ else if (i == compass) {
+ nb = ((MPLSensor*) mSensor)->readCompassEvents(data, count);
+ }
+/*
+ if (nb > 0) {
+ count -= nb;
+ nbEvents += nb;
+ data += nb;
+ mPollFds[i].revents = 0;
+ }
+ */
+ }
+ }
+ nb = ((MPLSensor*) mSensor)->executeOnData(data, count);
+ if (nb > 0) {
+ count -= nb;
+ nbEvents += nb;
+ data += nb;
+ mPollFds[mpl].revents = 0;
+ mPollFds[compass].revents = 0;
+ }
+ }
+
+ return nbEvents;
+}
+
+/******************************************************************************/
+
+static int poll__close(struct hw_device_t *dev)
+{
+ FUNC_LOG;
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ if (ctx) {
+ delete ctx;
+ }
+ return 0;
+}
+
+static int poll__activate(struct sensors_poll_device_t *dev,
+ int handle, int enabled)
+{
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ return ctx->activate(handle, enabled);
+}
+
+static int poll__setDelay(struct sensors_poll_device_t *dev,
+ int handle, int64_t ns)
+{
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ int s= ctx->setDelay(handle, ns);
+ return s;
+}
+
+static int poll__poll(struct sensors_poll_device_t *dev,
+ sensors_event_t* data, int count)
+{
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ return ctx->pollEvents(data, count);
+}
+
+/******************************************************************************/
+
+/** Open a new instance of a sensor device using name */
+static int open_sensors(const struct hw_module_t* module, const char* id,
+ struct hw_device_t** device)
+{
+ FUNC_LOG;
+ int status = -EINVAL;
+ sensors_poll_context_t *dev = new sensors_poll_context_t();
+
+ memset(&dev->device, 0, sizeof(sensors_poll_device_t));
+
+ dev->device.common.tag = HARDWARE_DEVICE_TAG;
+ dev->device.common.version = 0;
+ dev->device.common.module = const_cast<hw_module_t*>(module);
+ dev->device.common.close = poll__close;
+ dev->device.activate = poll__activate;
+ dev->device.setDelay = poll__setDelay;
+ dev->device.poll = poll__poll;
+
+ *device = &dev->device.common;
+ status = 0;
+
+ return status;
+}
diff --git a/libsensors_iio/software/build/android/common.mk b/libsensors_iio/software/build/android/common.mk new file mode 100644 index 0000000..b84a99c --- /dev/null +++ b/libsensors_iio/software/build/android/common.mk @@ -0,0 +1,68 @@ +# Use bash for additional echo fancyness +SHELL = /bin/bash + +#################################################################################################### +## defines + +## 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 +HALWRAPPER_LIB_NAME ?= androidhal + +## applications ## +SHARED_APP_SUFFIX = -shared +STATIC_APP_SUFFIX = -static + +#################################################################################################### +## includes and linker + +ANDROID_LINK = -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib +ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib + +ANDROID_INCLUDES = -I$(ANDROID_ROOT)/system/core/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/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/libsensors_iio/software/build/android/shared.mk b/libsensors_iio/software/build/android/shared.mk new file mode 100644 index 0000000..bc8548c --- /dev/null +++ b/libsensors_iio/software/build/android/shared.mk @@ -0,0 +1,74 @@ +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/console/linux/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/input_sub/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/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/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h new file mode 100644 index 0000000..fce28ae --- /dev/null +++ b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h @@ -0,0 +1,83 @@ +/** + * @brief Provides helpful file IO wrappers for use with sysfs. + * @details Based on Jonathan Cameron's @e iio_utils.h. + */ + +#ifndef _INV_SYSFS_UTILS_H_ +#define _INV_SYSFS_UTILS_H_ + +/** + * struct inv_sysfs_names_s - Files needed by user applications. + * @buffer: Ring buffer attached to FIFO. + * @enable: Turns on HW-to-ring buffer flow. + * @raw_data: Raw data from registers. + * @temperature: Temperature data from register. + * @fifo_rate: FIFO rate/ODR. + * @power_state: Power state (this is a five-star comment). + * @fsr: Full-scale range. + * @lpf: Digital low pass filter. + * @scale: LSBs / dps (or LSBs / Gs). + * @temp_scale: LSBs / degrees C. + * @temp_offset: Offset in LSBs. + */ +struct inv_sysfs_names_s { + + //Sysfs for ITG3500 & MPU6050 + const char *buffer; + const char *enable; + const char *raw_data; //Raw Gyro data + const char *temperature; + const char *fifo_rate; + const char *power_state; + const char *fsr; + const char *lpf; + const char *scale; //Gyro scale + const char *temp_scale; + const char *temp_offset; + const char *self_test; + //Starting Sysfs available for MPU6050 only + const char *accel_en; + const char *accel_fifo_en; + const char *accel_fs; + const char *clock_source; + const char *early_suspend_en; + const char *firmware_loaded; + const char *gyro_en; + const char *gyro_fifo_en; + const char *key; + const char *raw_accel; + const char *reg_dump; + const char *tap_on; + const char *dmp_firmware; +}; + +/* File IO. Typically won't be called directly by user application, but they'll + * be here for your enjoyment. + */ +int inv_sysfs_write(char *filename, long data); +int inv_sysfs_read(char *filename, long num_bytes, char *data); + +/* Helper APIs to extract specific data. */ +int inv_read_buffer(int fd, long *data, long long *timestamp); +int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, + long long *timestamp); +int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, + long long *timestamp); +int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data); +int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data); +int inv_read_scale(const struct inv_sysfs_names_s *names, float *data); +int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data); +int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data); +int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data); +int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data); +int inv_write_power_state(const struct inv_sysfs_names_s *names, char data); + +/* Scaled data. */ +int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, + long long *timestamp); +int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, + long long *timestamp); + +#endif /* #ifndef _INV_SYSFS_UTILS_H_ */ + + diff --git a/libsensors_iio/software/core/HAL/include/mlos.h b/libsensors_iio/software/core/HAL/include/mlos.h new file mode 100644 index 0000000..ce06b07 --- /dev/null +++ b/libsensors_iio/software/core/HAL/include/mlos.h @@ -0,0 +1,104 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include <stdio.h> +#endif + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(LINUX) || defined(__KERNEL__) +typedef unsigned int HANDLE; +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* - MLOSCreateFile defines. - */ + +#define MLOS_GENERIC_READ ((unsigned int)0x80000000) +#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) +#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) +#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) +#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) + + /* ---------- */ + /* - Enums. - */ + /* ---------- */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + +#ifndef __KERNEL__ +#include <string.h> + void *inv_malloc(unsigned int numBytes); + inv_error_t inv_free(void *ptr); + inv_error_t inv_create_mutex(HANDLE *mutex); + inv_error_t inv_lock_mutex(HANDLE mutex); + inv_error_t inv_unlock_mutex(HANDLE mutex); + FILE *inv_fopen(char *filename); + void inv_fclose(FILE *fp); + + // Binary Semaphore + inv_error_t inv_create_binary_semaphore(HANDLE *semaphore); + inv_error_t inv_destroy_binary_semaphore(HANDLE handle); + inv_error_t inv_increment_binary_semaphore(HANDLE handle); + inv_error_t inv_decrement_binary_semaphore(HANDLE handle, long timeout_ms); + + inv_error_t inv_destroy_mutex(HANDLE handle); + + void inv_sleep(int mSecs); + inv_time_t inv_get_tick_count(void); + + /* Kernel implmentations */ +#define GFP_KERNEL (0x70) + static inline void *kmalloc(size_t size, + unsigned int gfp_flags) + { + (void)gfp_flags; + return inv_malloc((unsigned int)size); + } + static inline void *kzalloc(size_t size, unsigned int gfp_flags) + { + void *tmp = inv_malloc((unsigned int)size); + (void)gfp_flags; + if (tmp) + memset(tmp, 0, size); + return tmp; + } + static inline void kfree(void *ptr) + { + inv_free(ptr); + } + static inline void msleep(long msecs) + { + inv_sleep(msecs); + } + static inline void udelay(unsigned long usecs) + { + inv_sleep((usecs + 999) / 1000); + } +#else +#include <linux/delay.h> +#endif + +#ifdef __cplusplus +} +#endif +#endif /* _MLOS_H */ diff --git a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c new file mode 100644 index 0000000..c45badd --- /dev/null +++ b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c @@ -0,0 +1,317 @@ +/** + * @brief Provides helpful file IO wrappers for use with sysfs. + * @details Based on Jonathan Cameron's @e iio_utils.h. + */ + +#include <string.h> +#include <stdlib.h> +#include <ctype.h> +#include <stdio.h> +#include <stdint.h> +#include <dirent.h> +#include <errno.h> +#include <unistd.h> +#include "inv_sysfs_utils.h" + +/* General TODO list: + * Select more reasonable string lengths or use fseek and malloc. + */ + +/** + * inv_sysfs_write() - Write an integer to a file. + * @filename: Path to file. + * @data: Value to write to file. + * Returns number of bytes written or a negative error code. + */ +int inv_sysfs_write(char *filename, long data) +{ + FILE *fp; + int count; + + if (!filename) + return -1; + fp = fopen(filename, "w"); + if (!fp) + return -errno; + count = fprintf(fp, "%ld", data); + fclose(fp); + return count; +} + +/** + * inv_sysfs_read() - Read a string from a file. + * @filename: Path to file. + * @num_bytes: Number of bytes to read. + * @data: Data from file. + * Returns number of bytes written or a negative error code. + */ +int inv_sysfs_read(char *filename, long num_bytes, char *data) +{ + FILE *fp; + int count; + + if (!filename) + return -1; + fp = fopen(filename, "r"); + if (!fp) + return -errno; + count = fread(data, 1, num_bytes, fp); + fclose(fp); + return count; +} + +/** + * inv_read_buffer() - Read data from ring buffer. + * @fd: File descriptor for buffer file. + * @data: Data in hardware units. + * @timestamp: Time when data was read from device. Use NULL if unsupported. + * Returns number of bytes written or a negative error code. + */ +int inv_read_buffer(int fd, long *data, long long *timestamp) +{ + char str[35]; + int count; + + count = read(fd, str, sizeof(str)); + if (!count) + return count; + if (!timestamp) + count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); + else + count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], + &data[2], timestamp); + + if (count < (timestamp?4:3)) + return -EAGAIN; + return count; +} + +/** + * inv_read_raw() - Read raw data. + * @names: Names of sysfs files. + * @data: Data in hardware units. + * @timestamp: Time when data was read from device. Use NULL if unsupported. + * Returns number of bytes written or a negative error code. + */ +int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, + long long *timestamp) +{ + char str[40]; + int count; + + count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str); + if (count < 0) + return count; + if (!timestamp) + count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); + else + count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], + &data[2], timestamp); + if (count < (timestamp?4:3)) + return -EAGAIN; + return count; +} + +/** + * inv_read_temperature_raw() - Read temperature. + * @names: Names of sysfs files. + * @data: Data in hardware units. + * @timestamp: Time when data was read from device. + * Returns number of bytes written or a negative error code. + */ +int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, + long long *timestamp) +{ + char str[25]; + int count; + + count = inv_sysfs_read((char*)names->temperature, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%hd%lld", &data[0], timestamp); + if (count < 2) + return -EAGAIN; + return count; +} + +/** + * inv_read_fifo_rate() - Read fifo rate. + * @names: Names of sysfs files. + * @data: Fifo rate. + * Returns number of bytes written or a negative error code. + */ +int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data) +{ + char str[8]; + int count; + + count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%hd", data); + if (count < 1) + return -EAGAIN; + return count; +} + +/** + * inv_read_power_state() - Read power state. + * @names: Names of sysfs files. + * @data: 1 if device is on. + * Returns number of bytes written or a negative error code. + */ +int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data) +{ + char str[2]; + int count; + + count = inv_sysfs_read((char*)names->power_state, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%hd", (short*)data); + if (count < 1) + return -EAGAIN; + return count; +} + +/** + * inv_read_scale() - Read scale. + * @names: Names of sysfs files. + * @data: 1 if device is on. + * Returns number of bytes written or a negative error code. + */ +int inv_read_scale(const struct inv_sysfs_names_s *names, float *data) +{ + char str[5]; + int count; + + count = inv_sysfs_read((char*)names->scale, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%f", data); + if (count < 1) + return -EAGAIN; + return count; +} + +/** + * inv_read_temp_scale() - Read temperature scale. + * @names: Names of sysfs files. + * @data: 1 if device is on. + * Returns number of bytes written or a negative error code. + */ +int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data) +{ + char str[4]; + int count; + + count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%hd", data); + if (count < 1) + return -EAGAIN; + return count; +} + +/** + * inv_read_temp_offset() - Read temperature offset. + * @names: Names of sysfs files. + * @data: 1 if device is on. + * Returns number of bytes written or a negative error code. + */ +int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data) +{ + char str[4]; + int count; + + count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%hd", data); + if (count < 1) + return -EAGAIN; + return count; +} + +/** + * inv_read_q16() - Get data as q16 fixed point. + * @names: Names of sysfs files. + * @data: 1 if device is on. + * @timestamp: Time when data was read from device. + * Returns number of bytes written or a negative error code. + */ +int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, + long long *timestamp) +{ + int count; + short raw[3]; + float scale; + count = inv_read_raw(names, (long*)raw, timestamp); + count += inv_read_scale(names, &scale); + data[0] = (long)(raw[0] * (65536.f / scale)); + data[1] = (long)(raw[1] * (65536.f / scale)); + data[2] = (long)(raw[2] * (65536.f / scale)); + return count; +} + +/** + * inv_read_q16() - Get temperature data as q16 fixed point. + * @names: Names of sysfs files. + * @data: 1 if device is on. + * @timestamp: Time when data was read from device. + * Returns number of bytes read or a negative error code. + */ +int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, + long long *timestamp) +{ + int count = 0; + short raw; + static short scale, offset; + static unsigned char first_read = 1; + + if (first_read) { + count += inv_read_temp_scale(names, &scale); + count += inv_read_temp_offset(names, &offset); + first_read = 0; + } + count += inv_read_temperature_raw(names, &raw, timestamp); + data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f); + + return count; +} + +/** + * inv_write_fifo_rate() - Write fifo rate. + * @names: Names of sysfs files. + * @data: Fifo rate. + * Returns number of bytes written or a negative error code. + */ +int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data) +{ + return inv_sysfs_write((char*)names->fifo_rate, (long)data); +} + +/** + * inv_write_buffer_enable() - Enable/disable buffer in /dev. + * @names: Names of sysfs files. + * @data: Fifo rate. + * Returns number of bytes written or a negative error code. + */ +int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data) +{ + return inv_sysfs_write((char*)names->enable, (long)data); +} + +/** + * inv_write_power_state() - Turn device on/off. + * @names: Names of sysfs files. + * @data: 1 to turn on. + * Returns number of bytes written or a negative error code. + */ +int inv_write_power_state(const struct inv_sysfs_names_s *names, char data) +{ + return inv_sysfs_write((char*)names->power_state, (long)data); +} + + diff --git a/libsensors_iio/software/core/HAL/linux/mlos_linux.c b/libsensors_iio/software/core/HAL/linux/mlos_linux.c new file mode 100644 index 0000000..75f062e --- /dev/null +++ b/libsensors_iio/software/core/HAL/linux/mlos_linux.c @@ -0,0 +1,194 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup MLOS + * @brief OS Interface. + * + * @{ + * @file mlos.c + * @brief OS Interface. +**/ + +/* ------------- */ +/* - Includes. - */ +/* ------------- */ + +#include <sys/time.h> +#include <unistd.h> +#include <pthread.h> +#include <stdlib.h> + +#include "stdint_invensense.h" + +#include "mlos.h" +#include <errno.h> + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param numBytes number of bytes + * @return pointer to allocated space +**/ +void *inv_malloc(unsigned int numBytes) +{ + // Allocate space. + void *allocPtr = malloc(numBytes); + return allocPtr; +} + + +/** + * @brief Free allocated space + * @param ptr pointer to space to deallocate + * @return error code. +**/ +inv_error_t inv_free(void *ptr) +{ + // Deallocate space. + free(ptr); + + return INV_SUCCESS; +} + + +/** + * @brief Mutex create function + * @param mutex pointer to mutex handle + * @return error code. + */ +inv_error_t inv_create_mutex(HANDLE *mutex) +{ + int res; + pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); + if(pm == NULL) + return INV_ERROR; + + res = pthread_mutex_init(pm, NULL); + if(res == -1) { + free(pm); + return INV_ERROR_OS_CREATE_FAILED; + } + + *mutex = (HANDLE)pm; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex lock function + * @param mutex Mutex handle + * @return error code. + */ +inv_error_t inv_lock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_lock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex unlock function + * @param mutex mutex handle + * @return error code. +**/ +inv_error_t inv_unlock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_unlock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief open file + * @param filename name of the file to open. + * @return error code. + */ +FILE *inv_fopen(char *filename) +{ + FILE *fp = fopen(filename,"r"); + return fp; +} + + +/** + * @brief close the file. + * @param fp handle to file to close. + * @return error code. + */ +void inv_fclose(FILE *fp) +{ + fclose(fp); +} + +/** + * @brief Close Handle + * @param handle handle to the resource. + * @return Zero if success, an error code otherwise. + */ +inv_error_t inv_destroy_mutex(HANDLE handle) +{ + int error; + pthread_mutex_t *pm = (pthread_mutex_t*)handle; + error = pthread_mutex_destroy(pm); + if (error) { + return errno; + } + free((void*) handle); + + return INV_SUCCESS;} + + +/** + * @brief Sleep function. + */ +void inv_sleep(int mSecs) +{ + usleep(mSecs*1000); +} + + +/** + * @brief get system's internal tick count. + * Used for time reference. + * @return current tick count. + */ +unsigned long inv_get_tick_count() +{ + struct timeval tv; + + if (gettimeofday(&tv, NULL) !=0) + return 0; + + return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); +} + + /**********************/ + /** @} */ /* defgroup */ +/**********************/ + diff --git a/libsensors_iio/software/core/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h new file mode 100644 index 0000000..9b29695 --- /dev/null +++ b/libsensors_iio/software/core/driver/include/linux/mpu.h @@ -0,0 +1,355 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file mpu.h + * @brief mpu definition + */ + +#ifndef __MPU_H_ +#define __MPU_H_ + +#ifdef __KERNEL__ +#include <linux/types.h> +#include <linux/ioctl.h> +#elif defined LINUX +#include <sys/ioctl.h> +#include <linux/types.h> +#else +#include "mltypes.h" +#endif + +struct mpu_read_write { + /* Memory address or register address depending on ioctl */ + __u16 address; + __u16 length; + __u8 *data; +}; + +enum mpuirq_data_type { + MPUIRQ_DATA_TYPE_MPU_DATA_READY_IRQ, + MPUIRQ_DATA_TYPE_MPU_FIFO_READY_IRQ, + MPUIRQ_DATA_TYPE_SLAVE_IRQ, + MPUIRQ_DATA_TYPE_PM_EVENT, + MPUIRQ_DATA_TYPE_NUM_TYPES, +}; + +/* User space PM event notification */ +#define MPU_PM_EVENT_SUSPEND_PREPARE (3) +#define MPU_PM_EVENT_POST_SUSPEND (4) + +/** + * struct mpuirq_data - structure to report what and when + * @interruptcount : The number of times this IRQ has occured since open + * @irqtime : monotonic time of the IRQ in ns + * @data_type : The type of this IRQ enum mpuirq_data_type + * @data : Data associated with this IRQ + */ +struct mpuirq_data { + __u32 interruptcount; + __s64 irqtime_ns; + __u32 data_type; + __s32 data; +}; + +enum ext_slave_config_key { + /* TODO: Remove these first six. */ + MPU_SLAVE_CONFIG_ODR_SUSPEND, + MPU_SLAVE_CONFIG_ODR_RESUME, + MPU_SLAVE_CONFIG_FSR_SUSPEND, + MPU_SLAVE_CONFIG_FSR_RESUME, + MPU_SLAVE_CONFIG_IRQ_SUSPEND, + MPU_SLAVE_CONFIG_IRQ_RESUME, + MPU_SLAVE_CONFIG_ODR, + MPU_SLAVE_CONFIG_FSR, + MPU_SLAVE_CONFIG_MOT_THS, + MPU_SLAVE_CONFIG_NMOT_THS, + MPU_SLAVE_CONFIG_MOT_DUR, + MPU_SLAVE_CONFIG_NMOT_DUR, + MPU_SLAVE_CONFIG_IRQ, + MPU_SLAVE_WRITE_REGISTERS, + MPU_SLAVE_READ_REGISTERS, + MPU_SLAVE_CONFIG_INTERNAL_REFERENCE, + /* AMI 306 specific config keys */ + MPU_SLAVE_PARAM, + MPU_SLAVE_WINDOW, + MPU_SLAVE_READWINPARAMS, + MPU_SLAVE_SEARCHOFFSET, + /* MPU3050 and MPU6050 Keys */ + MPU_SLAVE_INT_CONFIG, + MPU_SLAVE_EXT_SYNC, + MPU_SLAVE_FULL_SCALE, + MPU_SLAVE_LPF, + MPU_SLAVE_CLK_SRC, + MPU_SLAVE_DIVIDER, + MPU_SLAVE_DMP_ENABLE, + MPU_SLAVE_FIFO_ENABLE, + MPU_SLAVE_DMP_CFG1, + MPU_SLAVE_DMP_CFG2, + MPU_SLAVE_TC, + MPU_SLAVE_GYRO, + MPU_SLAVE_ADDR, + MPU_SLAVE_PRODUCT_REVISION, + MPU_SLAVE_SILICON_REVISION, + MPU_SLAVE_PRODUCT_ID, + MPU_SLAVE_GYRO_SENS_TRIM, + MPU_SLAVE_ACCEL_SENS_TRIM, + MPU_SLAVE_RAM, + /* -------------------------- */ + MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS +}; + +/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */ +enum ext_slave_config_irq_type { + MPU_SLAVE_IRQ_TYPE_NONE, + MPU_SLAVE_IRQ_TYPE_MOTION, + MPU_SLAVE_IRQ_TYPE_DATA_READY, +}; + +/* Structure for the following IOCTS's + * MPU_CONFIG_GYRO + * MPU_CONFIG_ACCEL + * MPU_CONFIG_COMPASS + * MPU_CONFIG_PRESSURE + * MPU_GET_CONFIG_GYRO + * MPU_GET_CONFIG_ACCEL + * MPU_GET_CONFIG_COMPASS + * MPU_GET_CONFIG_PRESSURE + * + * @key one of enum ext_slave_config_key + * @len length of data pointed to by data + * @apply zero if communication with the chip is not necessary, false otherwise + * This flag can be used to select cached data or to refresh cashed data + * cache data to be pushed later or push immediately. If true and the + * slave is on the secondary bus the MPU will first enger bypass mode + * before calling the slaves .config or .get_config funcion + * @data pointer to the data to confgure or get + */ +struct ext_slave_config { + __u8 key; + __u16 len; + __u8 apply; + void *data; +}; + +enum ext_slave_type { + EXT_SLAVE_TYPE_GYROSCOPE, + EXT_SLAVE_TYPE_ACCEL, + EXT_SLAVE_TYPE_COMPASS, + EXT_SLAVE_TYPE_PRESSURE, + /*EXT_SLAVE_TYPE_TEMPERATURE */ + + EXT_SLAVE_NUM_TYPES +}; +enum secondary_slave_type { + SECONDARY_SLAVE_TYPE_NONE, + SECONDARY_SLAVE_TYPE_ACCEL, + SECONDARY_SLAVE_TYPE_COMPASS, + SECONDARY_SLAVE_TYPE_PRESSURE, + + SECONDARY_SLAVE_TYPE_TYPES +}; + +enum ext_slave_id { + ID_INVALID = 0, + GYRO_ID_MPU3050, + GYRO_ID_MPU6050A2, + GYRO_ID_MPU6050B1, + GYRO_ID_MPU6050B1_NO_ACCEL, + GYRO_ID_ITG3500, + + ACCEL_ID_LIS331, + ACCEL_ID_LSM303DLX, + ACCEL_ID_LIS3DH, + ACCEL_ID_KXSD9, + ACCEL_ID_KXTF9, + ACCEL_ID_BMA150, + ACCEL_ID_BMA222, + ACCEL_ID_BMA250, + ACCEL_ID_ADXL34X, + ACCEL_ID_MMA8450, + ACCEL_ID_MMA845X, + ACCEL_ID_MPU6050, + + COMPASS_ID_AK8963, + COMPASS_ID_AK8975, + COMPASS_ID_AK8972, + COMPASS_ID_AMI30X, + COMPASS_ID_AMI306, + COMPASS_ID_YAS529, + COMPASS_ID_YAS530, + COMPASS_ID_HMC5883, + COMPASS_ID_LSM303DLH, + COMPASS_ID_LSM303DLM, + COMPASS_ID_MMC314X, + COMPASS_ID_HSCDTD002B, + COMPASS_ID_HSCDTD004A, + + PRESSURE_ID_BMA085, +}; + +#define INV_PROD_KEY(ver, rev) (ver * 100 + rev) + +enum ext_slave_endian { + EXT_SLAVE_BIG_ENDIAN, + EXT_SLAVE_LITTLE_ENDIAN, + EXT_SLAVE_FS8_BIG_ENDIAN, + EXT_SLAVE_FS16_BIG_ENDIAN, +}; + +enum ext_slave_bus { + EXT_SLAVE_BUS_INVALID = -1, + EXT_SLAVE_BUS_PRIMARY = 0, + EXT_SLAVE_BUS_SECONDARY = 1 +}; + + +/** + * struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050 + * slave devices + * + * @type: the type of slave device based on the enum ext_slave_type + * definitions. + * @irq: the irq number attached to the slave if any. + * @adapt_num: the I2C adapter number. + * @bus: the bus the slave is attached to: enum ext_slave_bus + * @address: the I2C slave address of the slave device. + * @orientation: the mounting matrix of the device relative to MPU. + * @irq_data: private data for the slave irq handler + * @private_data: additional data, user customizable. Not touched by the MPU + * driver. + * + * The orientation matricies are 3x3 rotation matricies + * that are applied to the data to rotate from the mounting orientation to the + * platform orientation. The values must be one of 0, 1, or -1 and each row and + * column should have exactly 1 non-zero value. + */ +struct ext_slave_platform_data { + __u8 type; + __u32 irq; + __u32 adapt_num; + __u32 bus; + __u8 address; + __s8 orientation[9]; + void *irq_data; + void *private_data; +}; + +struct fix_pnt_range { + __s32 mantissa; + __s32 fraction; +}; + +static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng) +{ + return (long)(rng.mantissa * 1000 + rng.fraction / 10); +} + +struct ext_slave_read_trigger { + __u8 reg; + __u8 value; +}; + +/** + * struct ext_slave_descr - Description of the slave device for programming. + * + * @suspend: function pointer to put the device in suspended state + * @resume: function pointer to put the device in running state + * @read: function that reads the device data + * @init: function used to preallocate memory used by the driver + * @exit: function used to free memory allocated for the driver + * @config: function used to configure the device + * @get_config:function used to get the device's configuration + * + * @name: text name of the device + * @type: device type. enum ext_slave_type + * @id: enum ext_slave_id + * @read_reg: starting register address to retrieve data. + * @read_len: length in bytes of the sensor data. Typically 6. + * @endian: byte order of the data. enum ext_slave_endian + * @range: full scale range of the slave ouput: struct fix_pnt_range + * @trigger: If reading data first requires writing a register this is the + * data to write. + * + * Defines the functions and information about the slave the mpu3050 and + * mpu6050 needs to use the slave device. + */ +struct ext_slave_descr { + int (*init) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*exit) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*suspend) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*resume) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*read) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + __u8 *data); + int (*config) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *config); + int (*get_config) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *config); + + char *name; + __u8 type; + __u8 id; + __u8 read_reg; + __u8 read_len; + __u8 endian; + struct fix_pnt_range range; + struct ext_slave_read_trigger *trigger; +}; + +/** + * struct mpu_platform_data - Platform data for the mpu driver + * @int_config: Bits [7:3] of the int config register. + * @level_shifter: 0: VLogic, 1: VDD + * @orientation: Orientation matrix of the gyroscope + * @sec_slave_type: secondary slave device type, can be compass, accel, etc + * @sec_slave_id: id of the secondary slave device + * @secondary_i2c_address: secondary device's i2c address + * @secondary_orientation: secondary device's orientation matrix + * + * Contains platform specific information on how to configure the MPU3050 to + * work on this platform. The orientation matricies are 3x3 rotation matricies + * that are applied to the data to rotate from the mounting orientation to the + * platform orientation. The values must be one of 0, 1, or -1 and each row and + * column should have exactly 1 non-zero value. + */ +struct mpu_platform_data { + __u8 int_config; + __u8 level_shifter; + __s8 orientation[9]; + enum secondary_slave_type sec_slave_type; + enum ext_slave_id sec_slave_id; + __u16 secondary_i2c_addr; + __s8 secondary_orientation[9]; + __u8 key[16]; +}; + +#endif /* __MPU_H_ */ diff --git a/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h new file mode 100644 index 0000000..74c49f3 --- /dev/null +++ b/libsensors_iio/software/core/driver/include/log.h @@ -0,0 +1,363 @@ +/* + $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 <stdarg.h> +#include "local_log_def.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 + +/* --------------------------------------------------------------------- */ + +/* + * 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/libsensors_iio/software/core/driver/include/mlinclude.h b/libsensors_iio/software/core/driver/include/mlinclude.h new file mode 100644 index 0000000..9725199 --- /dev/null +++ b/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/libsensors_iio/software/core/driver/include/mlmath.h b/libsensors_iio/software/core/driver/include/mlmath.h new file mode 100644 index 0000000..37194d6 --- /dev/null +++ b/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/libsensors_iio/software/core/driver/include/mlsl.h b/libsensors_iio/software/core/driver/include/mlsl.h new file mode 100644 index 0000000..12f2901 --- /dev/null +++ b/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/libsensors_iio/software/core/driver/include/mltypes.h b/libsensors_iio/software/core/driver/include/mltypes.h new file mode 100644 index 0000000..09eccce --- /dev/null +++ b/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/libsensors_iio/software/core/driver/include/stdint_invensense.h b/libsensors_iio/software/core/driver/include/stdint_invensense.h new file mode 100644 index 0000000..b8c2511 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/CMakeLists.txt b/libsensors_iio/software/core/mllite/CMakeLists.txt new file mode 100644 index 0000000..8650553 --- /dev/null +++ b/libsensors_iio/software/core/mllite/CMakeLists.txt @@ -0,0 +1,39 @@ +## CMakeLists for mlsdk/mldmp + +project(mldmp C) + +if (NOT CMAKE_BUILD_ENGINEERING) + +# # just copy the library from mldmp/mpl/$PLATFORM to mldmp/ +# if (CMAKE_SYSTEM_NAME MATCHES Windows) +# execute_process( +# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib +# ) +# elseif (CMAKE_SYSTEM_NAME MATCHES Android) +# execute_process( +# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a +# ) +# elseif (CMAKE_SYSTEM_NAME MATCHES Linux) +# message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a") +# execute_process( +# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a +# ) +# elseif (CMAKE_SYSTEM_NAME MATCHES Catalina) +# execute_process( +# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a +# ) +# endif() +# # better way that doesn't work for now +# # add_custom_command( +# # TARGET mpl +# # PRE_BUILD +# # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR} +# # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}" +# # ) + +else (NOT CMAKE_BUILD_ENGINEERING) + + set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) + include(Engineering) + +endif (NOT CMAKE_BUILD_ENGINEERING) diff --git a/libsensors_iio/software/core/mllite/Engineering.cmake b/libsensors_iio/software/core/mllite/Engineering.cmake new file mode 100644 index 0000000..42f2429 --- /dev/null +++ b/libsensors_iio/software/core/mllite/Engineering.cmake @@ -0,0 +1,150 @@ +## Engineering CMakeLists for software/core/mllite + +include_directories( + . + ${SOLUTION_SOURCE_DIR}/core/mpl + ${SOLUTION_SOURCE_DIR}/core/HAL + ${SOLUTION_SOURCE_DIR}/driver/include +) + +add_definitions ( + -DINV_INCLUDE_LEGACY_HEADERS +) + +if (CMAKE_SYSTEM_NAME MATCHES Android) + + include_directories( + ${SOLUTION_SOURCE_DIR}/core/mllite/linux + ${SOLUTION_SOURCE_DIR}/driver/include/linux + ) + add_definitions( + -DLINUX + -D_REENTRANT + ) + set (HEADERS + ${HEADERS} + linux/mlos.h + linux/ml_stored_data.h + linux/ml_load_dmp.h + linux/ml_sysfs_helper.h + ) + set (SOURCES + ${SOURCES} + linux/mlos_linux.c + linux/ml_stored_data.c + linux/ml_load_dmp.c + linux/ml_sysfs_helper.c + ) + +elseif (CMAKE_SYSTEM_NAME MATCHES Linux) + + add_definitions( + -DLINUX + -D_REENTRANT + ) + set (HEADERS + ${HEADERS} + ) + set (SOURCES + ${SOURCES} + ) + +elseif (CMAKE_SYSTEM_NAME MATCHES Windows) + + add_definitions( + -DAIO + -DUART_COM + -D_CRT_SECURE_NO_WARNINGS + -D_CRT_SECURE_NO_DEPRECATE + ) + set (HEADERS + ${HEADERS} + ) + set (SOURCES + ${SOURCES} + ) + +endif () + +set (HEADERS + ${HEADERS} + data_builder.h + hal_outputs.h + message_layer.h + ml_math_func.h + mpl.h + results_holder.h + start_manager.h + storage_manager.h +) +set (SOURCES + ${SOURCES} + data_builder.c + hal_outputs.c + message_layer.c + ml_math_func.c + mpl.c + results_holder.c + start_manager.c + storage_manager.c +) + +# coveniently add this file to the resources for easy access within IDEs +set (RESOURCES + Engineering.cmake +) + +if (CMAKE_TESTING_SUPPORT) + + message("Including Testing support") + include_directories( + ${SOLUTION_SOURCE_DIR}/mltools/debugsupport + ) + add_definitions( + -DSELF_VERIFICATION + ) + set( + HEADERS + ${HEADERS} + ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h + ) + set( + SOURCES + ${SOURCES} + ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c + ) + +endif () + +############ +## TARGET ## +############ +add_library( + mllite STATIC + ${SOURCES} + ${HEADERS} + ${RESOURCES} +) +set_target_properties( + mllite + PROPERTIES CLEAN_DIRECT_OUTPUT 1 +) + +if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)") + + add_library( + mllite_shared SHARED + ${SOURCES} + ${HEADERS} + ${RESOURCES} + ) + FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite") + + install ( + TARGETS mllite_shared + DESTINATION lib + ) + +endif () + + diff --git a/libsensors_iio/software/core/mllite/build/android/shared.mk b/libsensors_iio/software/core/mllite/build/android/shared.mk new file mode 100644 index 0000000..2ee2e20 --- /dev/null +++ b/libsensors_iio/software/core/mllite/build/android/shared.mk @@ -0,0 +1,91 @@ +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 += -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 += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-shared,-Bsymbolic +LFLAGS += $(ANDROID_LINK) +LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## 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/libsensors_iio/software/core/mllite/build/android/static.mk b/libsensors_iio/software/core/mllite/build/android/static.mk new file mode 100644 index 0000000..6ad45de --- /dev/null +++ b/libsensors_iio/software/core/mllite/build/android/static.mk @@ -0,0 +1,110 @@ +MLLITE_LIB_NAME = mllite +LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +OBJFOLDER = $(CURDIR)/obj + +CROSS = arm-none-linux-gnueabi- +COMP= $(CROSS)gcc +LINK= $(CROSS)ar cr + +MLLITE_DIR = $(MLSDK_ROOT)/mllite +MPL_DIR = $(MLSDK_ROOT)/mldmp +MLPLATFORM_DIR = $(MLSDK_ROOT)/platform + +include $(MLSDK_ROOT)/Android-common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += -Wall -fpic +CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT -DLINUX -DANDROID +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MLPLATFORM_DIR)/include +CFLAGS += -I$(MLSDK_ROOT)/mlutils +CFLAGS += -I$(MLSDK_ROOT)/mlapps/common +CFLAGS += $(MLSDK_INCLUDES) +CFLAGS += $(MLSDK_DEFINES) + +VPATH += $(MLLITE_DIR) +VPATH += $(MLSDK_ROOT)/mlutils +VPATH += $(MLLITE_DIR)/accel +VPATH += $(MLLITE_DIR)/compass +VPATH += $(MLLITE_DIR)/pressure +VPATH += $(MLLITE_DIR)/mlapps/common + +#################################################################################################### +## sources + +ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) + +ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c +ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c +ML_SOURCES += $(MLLITE_DIR)/accel.c +ML_SOURCES += $(MLLITE_DIR)/compass.c +ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c +ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c +ML_SOURCES += $(MLLITE_DIR)/key0_96.c +ML_SOURCES += $(MLLITE_DIR)/pressure.c +ML_SOURCES += $(MLLITE_DIR)/ml.c +ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c +ML_SOURCES += $(MLLITE_DIR)/ml_init.c +ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c +ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c +ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c +ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c +ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c +ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c +ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c +ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c +ML_SOURCES += $(MLLITE_DIR)/mldl.c +ML_SOURCES += $(MLLITE_DIR)/mldmp.c +ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c +ML_SOURCES += $(MLLITE_DIR)/mlstates.c +ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c +ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c +ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c +ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c +ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c +ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c +ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c +ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c +ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c +ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c +ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c +ifeq ($(MPU_NAME),MPU3050) +ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c +else +ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c +endif + +ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) +ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall + +all: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n") + $(LINK) $(LIBRARY) $(ML_OBJS_DST) + $(CROSS)ranlib $(LIBRARY) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/libsensors_iio/software/core/mllite/build/filelist.mk b/libsensors_iio/software/core/mllite/build/filelist.mk new file mode 100644 index 0000000..011120c --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c new file mode 100644 index 0000000..f70be7c --- /dev/null +++ b/libsensors_iio/software/core/mllite/data_builder.c @@ -0,0 +1,1162 @@ +/* + $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 0 /* Use 0 to turn on MPL_LOGV output */ + +#include "ml_math_func.h" +#include "data_builder.h" +#include "mlmath.h" +#include "storage_manager.h" +#include "message_layer.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL" + +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_db_save_t { + /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ + long compass_bias[3]; + /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ + long gyro_bias[3]; + /** Temperature when *gyro_bias was stored. */ + long gyro_temp; + /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ + long accel_bias[3]; + /** Temperature when accel bias was stored. */ + long accel_temp; + long gyro_temp_slope[3]; +}; + +struct inv_data_builder_t { + int num_cb; + struct process_t process[INV_MAX_DATA_CB]; + struct inv_db_save_t save; + int compass_disturbance; +#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; + +/** Change this key if the data being stored by this file changes */ +#define INV_DB_SAVE_KEY 53394 + +#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 + +/** 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)); + 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; +} + +/** 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)); + return inv_register_load_store(inv_db_load_func, inv_db_save_func, + sizeof(inv_data_builder.save), + INV_DB_SAVE_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() +{ + 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) +{ + sensor->sensitivity = sensitivity; + sensor->orientation = orientation; +} +void inv_get_quaternion_transformation(int orientation, long *q) +{ + long s = 759250125; // sqrt(.5)*2^30 + + switch (orientation) + { + case 0x70: + q[0] = 759250125; + q[1] = 759250125; + q[2] = 0; + q[3] = 0; + break; + case 0x10a: + q[0] = 759250125; + q[1] = 0; + q[2] = 759250125; + q[3] = 0; + break; + case 0x85: + q[0] = 759250125; + q[1] = 0; + q[2] = 0; + q[3] = 759250125; + break; + case 0x181: + q[0] = 0; + q[1] = 759250125; + q[2] = 759250125; + q[3] = 0; + break; + case 0x2a: + q[0] = 0; + q[1] = 759250125; + q[2] = 0; + q[3] = 759250125; + break; + case 0x54: + q[0] = 0; + q[1] = 0; + q[2] = 759250125; + q[3] = 759250125; + break; + case 0x150: + q[0] = 759250125; + q[1] = -759250125; + q[2] = 0; + q[3] = 0; + break; + case 0xe: + q[0] = 759250125; + q[1] = 0; + q[2] = -759250125; + q[3] = 0; + break; + case 0xa1: + q[0] = 759250125; + q[1] = 0; + q[2] = 0; + q[3] = -759250125; + break; + case 0x1a5: + q[0] = 0; + q[1] = 759250125; + q[2] = -759250125; + q[3] = 0; + break; + case 0x12e: + q[0] = 0; + q[1] = 759250125; + q[2] = 0; + q[3] = -759250125; + break; + case 0x174: + q[0] = 0; + q[1] = 0; + q[2] = 759250125; + q[3] = -759250125; + break; + + + case 0x88: + q[0] = 1073741824; + q[1] = 0; + q[2] = 0; + q[3] = 0; + break; + + + case 0x1a8: + q[0] = 0; + q[1] = 1073741824; + q[2] = 0; + q[3] = 0; + break; + + case 0x18c: + q[0] = 0; + q[1] = 0; + q[2] = 1073741824; + q[3] = 0; + break; + + case 0xac: + q[0] = 0; + q[1] = 0; + q[2] = 0; + q[3] = 1073741824; + break; + + default: + break; + } +} +/** Sets the Orientation and Sensitivity of the gyro data. +* @param[in] orientation A scalar defining the transformation from chip mounting +* to the body frame. The function inv_orientation_matrix_to_scalar() +* 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); + } +} + +/** 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); + } +} +/** 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; + } + } + 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; + } + 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. +* @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] << 16; + raw32[1] = (long)sensor->raw[1] << 16; + raw32[2] = (long)sensor->raw[2] << 16; + + raw32[0] -= bias[0]; + raw32[1] -= bias[1]; + raw32[2] -= bias[2]; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, 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)); + } +} + +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_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 accel bias. +* @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. +*/ +void inv_set_accel_bias(const long *bias, int accuracy) +{ + if (bias) { + if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) { + memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias)); + inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); + } + } + sensors.accel.accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + +/** Sets the accel bias with control over which axis. +* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame +* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. +* @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_bias[0] = bias[0]; + } + if (mask & 2){ + inv_data_builder.save.accel_bias[1] = bias[1]; + } + if (mask & 4){ + inv_data_builder.save.accel_bias[2] = bias[2]; + } + + inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); + } + sensors.accel.accuracy = accuracy; +} + + +/** Sets the gyro bias +* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. +* Length 3. +* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. +*/ +void inv_set_gyro_bias(const long *bias, int accuracy) +{ + if (bias != NULL) { + if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { + memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); + inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); + } + } + sensors.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 = 1769472L; + inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); +} + +/* 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. + * @param[in] temp + * Tempearature in degrees C. + */ +void inv_get_gyro_bias(long *bias, long *temp) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save.gyro_bias, + sizeof(inv_data_builder.save.gyro_bias)); + if (temp != NULL) + temp[0] = inv_data_builder.save.gyro_temp; +} + +/** Get Accel Bias +* @param[out] bias Accel bias where +* @param[out] temp Temperature where 1 C = 2^16 +*/ +void inv_get_accel_bias(long *bias, long *temp) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save.accel_bias, + sizeof(inv_data_builder.save.accel_bias)); + if (temp != NULL) + temp[0] = inv_data_builder.save.accel_temp; +} + +/** + * 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_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; + } + 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.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) { + sensors.compass.raw[0] = (short)compass[0]; + sensors.compass.raw[1] = (short)compass[1]; + sensors.compass.raw[2] = (short)compass[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; + } + 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 +* @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 + + memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); + sensors.quat.timestamp = timestamp; + sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.quat.status |= (INV_BIAS_APPLIED & status); + + 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() +{ + 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() +{ + 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) +{ + 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() +{ + 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; + int mode; + +#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 + mode = 0; + if (sensors.gyro.status & INV_NEW_DATA) + mode |= INV_GYRO_NEW; + if (sensors.accel.status & INV_NEW_DATA) + mode |= INV_ACCEL_NEW; + if (sensors.compass.status & INV_NEW_DATA) + mode |= INV_MAG_NEW; + if (sensors.temp.status & INV_NEW_DATA) + mode |= INV_TEMP_NEW; + if (sensors.quat.status & INV_QUAT_NEW) + mode |= INV_QUAT_NEW; + + first_error = INV_SUCCESS; + + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if (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; +} + +/** 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; + } +} + +/** 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 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; +} + + +/** + * @} + */ diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h new file mode 100644 index 0000000..b2d0881 --- /dev/null +++ b/libsensors_iio/software/core/mllite/data_builder.h @@ -0,0 +1,224 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_DATA_BUILDER_H__ +#define INV_DATA_BUILDER_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 + +/** 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 + +#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_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_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]; + /** 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; + long sample_rate_us; + long sample_rate_ms; +}; + +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; + /** 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 + +} inv_rd_dbg_states; + +/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/ +#define INV_MAX_DATA_CB 20 + +#ifdef INV_PLAYBACK_DBG +#include <stdio.h> +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); + +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_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, int accuracy); +void inv_set_accel_bias(const long *bias, int accuracy); +void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); + +void inv_get_gyro_bias(long *bias, long *temp); +void inv_get_accel_bias(long *bias, long *temp); + +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_compass_set(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); + +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 __cplusplus +} +#endif + +#endif /* INV_DATA_BUILDER_H__ */ diff --git a/libsensors_iio/software/core/mllite/dmpconfig.txt b/libsensors_iio/software/core/mllite/dmpconfig.txt new file mode 100644 index 0000000..4643ed5 --- /dev/null +++ b/libsensors_iio/software/core/mllite/dmpconfig.txt @@ -0,0 +1,3 @@ +major version = 1 +minor version = 0 +startAddr = 0 diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c new file mode 100644 index 0000000..1cd3b81 --- /dev/null +++ b/libsensors_iio/software/core/mllite/hal_outputs.c @@ -0,0 +1,425 @@ +/* + $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. + */ +#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" + +struct hal_output_t { + int accuracy_mag; /**< Compass accuracy */ + int accuracy_gyro; /**< Gyro Accuracy */ + int accuracy_accel; /**< Accel Accuracy */ + inv_time_t nav_timestamp; + inv_time_t gam_timestamp; + inv_time_t accel_timestamp; + long nav_quat[4]; + int gyro_status; + int accel_status; + int compass_status; + int nine_axis_status; +}; + +static struct hal_output_t hal_out; + +/** 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; + 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_get_accel_set(accel, accuracy, timestamp); + 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 hal_out.nine_axis_status; +} + +/** 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]; + int status; + + *accuracy = hal_out.accuracy_mag; + *timestamp = hal_out.nav_timestamp; + 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; + if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) + status = 1; + else + status = 0; + return status; +} + +/** Gyroscope data (rad/s) in body frame. +* @param[out] values Rotation Rate in rad/sec. +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to +* 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) +{ + /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. + * So this is: pi / 2^16 / 180 */ +#define GYRO_CONVERSION 2.66316109007924e-007f + long gyro[3]; + int status; + + 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; +} + +/** +* 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 4. +* @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_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + *accuracy = hal_out.accuracy_mag; + *timestamp = hal_out.nav_timestamp; + + if (hal_out.nav_quat[0] >= 0) { + values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; + values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; + values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; + values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; + } else { + values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; + values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; + values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; + values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; + } + values[4] = inv_get_heading_confidence_interval(); + + return hal_out.nine_axis_status; +} + + +/** 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; + /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. + * So this is: 1 / 2^16*/ +#define COMPASS_CONVERSION 1.52587890625e-005f + long compass[3]; + inv_get_compass_set(compass, accuracy, timestamp); + values[0] = compass[0] * COMPASS_CONVERSION; + values[1] = compass[1] * COMPASS_CONVERSION; + values[2] = compass[2] * COMPASS_CONVERSION; + if (hal_out.compass_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + + +/** 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) +{ + long t1, t2, t3; + long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; + + *accuracy = hal_out.accuracy_mag; + *timestamp = hal_out.nav_timestamp; + + q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]); + q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]); + q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]); + q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]); + q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]); + q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]); + q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]); + q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]); + q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]); + q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]); + + /* X component of the Ybody axis in World frame */ + t1 = q12 - q03; + + /* Y component of the Ybody axis in World frame */ + t2 = q22 + q00 - (1L << 30); + values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; + if (values[0] < 0) + values[0] += 360; + + /* Z component of the Ybody axis in World frame */ + t3 = q23 + q01; + values[1] = + -atan2f((float) t3, + sqrtf((float) t1 * t1 + + (float) t2 * t2)) * 180.f / (float) M_PI; + /* Z component of the Zbody axis in World frame */ + t2 = q33 + q00 - (1L << 30); + if (t2 < 0) { + if (values[1] >= 0) + values[1] = 180.f - values[1]; + else + values[1] = -180.f - values[1]; + } + + /* X component of the Xbody axis in World frame */ + t1 = q11 + q00 - (1L << 30); + /* Y component of the Xbody axis in World frame */ + t2 = q12 + q03; + /* Z component of the Xbody axis in World frame */ + t3 = q13 - q02; + //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI; + + values[2] = + -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) * + 180.f / (float) M_PI - 90); + if (values[2] >= 90) + values[2] = 180 - values[2]; + + if (values[2] < -90) + values[2] = -180 - values[2]; + + return hal_out.nine_axis_status; +} + +/** Main callback to generate HAL outputs. Typically not called by library users. +* @param[in] sensor_cal Input variable to take sensor data whenever there is new +* 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; + (void) sensor_cal; + inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag, + &hal_out.nav_timestamp); + hal_out.gyro_status = sensor_cal->gyro.status; + hal_out.accel_status = sensor_cal->accel.status; + hal_out.compass_status = sensor_cal->compass.status; + + // 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; + } + + switch (use_sensor) { + default: + case 0: + hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; + hal_out.nav_timestamp = sensor_cal->gyro.timestamp; + 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; + } + + 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); + return result; +} + +/** 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) +{ + memset(&hal_out, 0, sizeof(hal_out)); + 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/libsensors_iio/software/core/mllite/hal_outputs.h b/libsensors_iio/software/core/mllite/hal_outputs.h new file mode 100644 index 0000000..ed4cb65 --- /dev/null +++ b/libsensors_iio/software/core/mllite/hal_outputs.h @@ -0,0 +1,43 @@ +/* + $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_magnetic_field(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); + + 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); + +#ifdef __cplusplus +} +#endif + +#endif // INV_HAL_OUTPUTS_H__ diff --git a/libsensors_iio/software/core/mllite/invensense.h b/libsensors_iio/software/core/mllite/invensense.h new file mode 100644 index 0000000..fb8e12b --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c new file mode 100644 index 0000000..649b917 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h new file mode 100644 index 0000000..45a35f9 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c new file mode 100644 index 0000000..f0f078c --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -0,0 +1,281 @@ +/* + $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 3060 + +static const unsigned char dmpMemory[DMP_CODE_SIZE] = { + /* bank # 0 */ + 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, + 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, + 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, + 0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, + 0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, + 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, + 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, + 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, + 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, + 0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, + 0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, + /* bank # 1 */ + 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, + 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, + 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, + 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, + 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, + 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, + 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, + /* bank # 2 */ + 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x44, 0x00, 0x05, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xc0, 0x00, 0x00, 0x01, 0x3f, 0xff, 0xff, 0xff, 0x40, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, + 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, + 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, + 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, 0x06, 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, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, + 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, 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, 0x00, + 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, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, + 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, + 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, + 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, 0xca, 0xf1, + 0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, 0x99, 0x2c, + 0x54, 0x7c, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, + 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, + 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, + 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, + 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, + 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, + 0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, + 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, + 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, + 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, + 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, + /* bank # 5 */ + 0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, + 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, + 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, + 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, + 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, + 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, + 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, + 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, + 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, + 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, + 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, + 0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, + 0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, + 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, + 0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, + 0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, + /* bank # 6 */ + 0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, + 0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, + 0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, + 0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, + 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, + 0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, + 0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, + 0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, + 0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, + 0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, + 0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, + 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, + 0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, + 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, + 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, + 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, + /* bank # 7 */ + 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, + 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, + 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, + 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, + 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, + 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, + 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, + 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, + 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, + 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, + 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, + 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, + 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, + 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, + 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, + 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, + /* bank # 8 */ + 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, + 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, + 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, + 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, + 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, + 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, + 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 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, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, + 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, + 0x49, 0x60, 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, + /* bank # 9 */ + 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, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, + 0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, + 0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, + 0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, + 0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, + 0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, + 0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, + 0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, + 0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, + 0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, + 0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, + 0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, + 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, + /* bank # 10 */ + 0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, + 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, + 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, + 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, + 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, + 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, + 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, + 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x8b, 0x93, 0xf2, 0x02, 0x02, 0xd1, 0xab, 0xda, + 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, + 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, + 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, + 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, + 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 0x2c, 0x87, + 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, + /* bank # 11 */ + 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28, + 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2, + 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 0x80, 0xd9, + 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 0x2c, 0xd8, + 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c, + 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 0x0c, 0xd8, + 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 0xff, 0xd8, + 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8, + 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x87, 0x31, + 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0xa8, 0x82, + 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 0xf1, 0xa6, + 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0x2a, + 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c, + 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde, + 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9, + 0x00, 0xd8, 0xf1, 0xff +}; + +#define DMP_VERSION (dmpMemory) + +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; +} + +/** + * @} + */ diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h new file mode 100644 index 0000000..4d98692 --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h @@ -0,0 +1,33 @@ +/* + $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); + +#ifdef __cplusplus +} +#endif +#endif /* INV_LOAD_DMP_H */ diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c new file mode 100644 index 0000000..c5cf2e6 --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c @@ -0,0 +1,353 @@ +/* + $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. + */ + +#include <stdio.h> + +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-storeload" + + +#include "ml_stored_data.h" +#include "storage_manager.h" +#include "log.h" +#include "mlos.h" + +#define LOADCAL_DEBUG 0 +#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 *cal, size_t len) +{ + FILE *fp; + int bytesRead; + inv_error_t result = INV_SUCCESS; + + fp = fopen(MLCAL_FILE,"rb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesRead = fread(cal, 1, len, fp); + if (bytesRead != len) { + MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", + bytesRead, len); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } + else { + MPL_LOGI("Bytes read = %d", bytesRead); + } + +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_LOGI("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_LOGI("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_LOGI("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; + inv_error_t result = 0; + size_t length; + + inv_get_mpl_state_size(&length); + if (length <= 0) { + MPL_LOGE("Could not get file calibration length - " + "error %d - aborting\n", result); + return result; + } + + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + + result = inv_read_cal(calData, length); + if(result != INV_SUCCESS) { + MPL_LOGE("Could not load cal file - " + "aborting\n"); + } + + result = inv_load_mpl_states(calData, length); + 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 INV_SUCCESS; +} + +/** + * @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_LOGI("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_LOGE("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 INV_SUCCESS; +} + +/** + * @} + */ diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h new file mode 100644 index 0000000..336f081 --- /dev/null +++ b/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 *cal, size_t len); +inv_error_t inv_write_cal(unsigned char *cal, size_t len); +inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len); +inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len); + +/* + 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/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c new file mode 100644 index 0000000..5636a02 --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c @@ -0,0 +1,416 @@ +#include <string.h> +#include <stdio.h> +#include "ml_sysfs_helper.h" +#include <dirent.h> +#include <ctype.h> +#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" + +#define CHIP_NUM 4 +enum PROC_SYSFS_CMD { + CMD_GET_SYSFS_PATH, + CMD_GET_DMP_PATH, + 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[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"}; +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" + +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) { + printf("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[100], 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[0])] = '\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; +} +/** + * @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, 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, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h new file mode 100644 index 0000000..eb285c5 --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h @@ -0,0 +1,36 @@ +/* + $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); +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/libsensors_iio/software/core/mllite/linux/mlos.h b/libsensors_iio/software/core/mllite/linux/mlos.h new file mode 100644 index 0000000..287025f --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/mlos.h @@ -0,0 +1,98 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include <stdio.h> +#endif + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(LINUX) || defined(__KERNEL__) +typedef unsigned int HANDLE; +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* - MLOSCreateFile defines. - */ + +#define MLOS_GENERIC_READ ((unsigned int)0x80000000) +#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) +#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) +#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) +#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) + + /* ---------- */ + /* - Enums. - */ + /* ---------- */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + +#ifndef __KERNEL__ +#include <string.h> + void *inv_malloc(unsigned int numBytes); + inv_error_t inv_free(void *ptr); + inv_error_t inv_create_mutex(HANDLE *mutex); + inv_error_t inv_lock_mutex(HANDLE mutex); + inv_error_t inv_unlock_mutex(HANDLE mutex); + FILE *inv_fopen(char *filename); + void inv_fclose(FILE *fp); + + 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/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/libsensors_iio/software/core/mllite/linux/mlos_linux.c new file mode 100644 index 0000000..75f062e --- /dev/null +++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c @@ -0,0 +1,194 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup MLOS + * @brief OS Interface. + * + * @{ + * @file mlos.c + * @brief OS Interface. +**/ + +/* ------------- */ +/* - Includes. - */ +/* ------------- */ + +#include <sys/time.h> +#include <unistd.h> +#include <pthread.h> +#include <stdlib.h> + +#include "stdint_invensense.h" + +#include "mlos.h" +#include <errno.h> + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param numBytes number of bytes + * @return pointer to allocated space +**/ +void *inv_malloc(unsigned int numBytes) +{ + // Allocate space. + void *allocPtr = malloc(numBytes); + return allocPtr; +} + + +/** + * @brief Free allocated space + * @param ptr pointer to space to deallocate + * @return error code. +**/ +inv_error_t inv_free(void *ptr) +{ + // Deallocate space. + free(ptr); + + return INV_SUCCESS; +} + + +/** + * @brief Mutex create function + * @param mutex pointer to mutex handle + * @return error code. + */ +inv_error_t inv_create_mutex(HANDLE *mutex) +{ + int res; + pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); + if(pm == NULL) + return INV_ERROR; + + res = pthread_mutex_init(pm, NULL); + if(res == -1) { + free(pm); + return INV_ERROR_OS_CREATE_FAILED; + } + + *mutex = (HANDLE)pm; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex lock function + * @param mutex Mutex handle + * @return error code. + */ +inv_error_t inv_lock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_lock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex unlock function + * @param mutex mutex handle + * @return error code. +**/ +inv_error_t inv_unlock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_unlock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief open file + * @param filename name of the file to open. + * @return error code. + */ +FILE *inv_fopen(char *filename) +{ + FILE *fp = fopen(filename,"r"); + return fp; +} + + +/** + * @brief close the file. + * @param fp handle to file to close. + * @return error code. + */ +void inv_fclose(FILE *fp) +{ + fclose(fp); +} + +/** + * @brief Close Handle + * @param handle handle to the resource. + * @return Zero if success, an error code otherwise. + */ +inv_error_t inv_destroy_mutex(HANDLE handle) +{ + int error; + pthread_mutex_t *pm = (pthread_mutex_t*)handle; + error = pthread_mutex_destroy(pm); + if (error) { + return errno; + } + free((void*) handle); + + return INV_SUCCESS;} + + +/** + * @brief Sleep function. + */ +void inv_sleep(int mSecs) +{ + usleep(mSecs*1000); +} + + +/** + * @brief get system's internal tick count. + * Used for time reference. + * @return current tick count. + */ +unsigned long inv_get_tick_count() +{ + struct timeval tv; + + if (gettimeofday(&tv, NULL) !=0) + return 0; + + return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); +} + + /**********************/ + /** @} */ /* defgroup */ +/**********************/ + diff --git a/libsensors_iio/software/core/mllite/message_layer.c b/libsensors_iio/software/core/mllite/message_layer.c new file mode 100644 index 0000000..8317957 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/message_layer.h b/libsensors_iio/software/core/mllite/message_layer.h new file mode 100644 index 0000000..df0c0e2 --- /dev/null +++ b/libsensors_iio/software/core/mllite/message_layer.h @@ -0,0 +1,35 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_MESSAGE_LAYER_H__
+#define INV_MESSAGE_LAYER_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /* Level 0 Type Messages */
+ /** A motion event has occured */
+#define INV_MSG_MOTION_EVENT (0x01)
+ /** A no motion event has occured */
+#define INV_MSG_NO_MOTION_EVENT (0x02)
+ /** A setting of the gyro bias has occured */
+#define INV_MSG_NEW_GB_EVENT (0x04)
+ /** A setting of the compass bias has occured */
+#define INV_MSG_NEW_CB_EVENT (0x08)
+ /** A setting of the accel bias has occured */
+#define INV_MSG_NEW_AB_EVENT (0x10)
+
+ void inv_set_message(long set, long clear, int level);
+ long inv_get_message_level_0(int clear);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MESSAGE_LAYER_H__
diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c new file mode 100644 index 0000000..86c4b41 --- /dev/null +++ b/libsensors_iio/software/core/mllite/ml_math_func.c @@ -0,0 +1,660 @@ +/* + $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]); +} +/** + * @} + */ diff --git a/libsensors_iio/software/core/mllite/ml_math_func.h b/libsensors_iio/software/core/mllite/ml_math_func.h new file mode 100644 index 0000000..916de0a --- /dev/null +++ b/libsensors_iio/software/core/mllite/ml_math_func.h @@ -0,0 +1,108 @@ +/* + $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 + + 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); + +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/libsensors_iio/software/core/mllite/mlmath.c b/libsensors_iio/software/core/mllite/mlmath.c new file mode 100644 index 0000000..5eb4264 --- /dev/null +++ b/libsensors_iio/software/core/mllite/mlmath.c @@ -0,0 +1,68 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/******************************************************************************* + * + * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#include <math.h> + +double ml_asin(double x) +{ + return asin(x); +} + +double ml_atan(double x) +{ + return atan(x); +} + +double ml_atan2(double x, double y) +{ + return atan2(x, y); +} + +double ml_log(double x) +{ + return log(x); +} + +double ml_sqrt(double x) +{ + return sqrt(x); +} + +double ml_ceil(double x) +{ + return ceil(x); +} + +double ml_floor(double x) +{ + return floor(x); +} + +double ml_cos(double x) +{ + return cos(x); +} + +double ml_sin(double x) +{ + return sin(x); +} + +double ml_acos(double x) +{ + return acos(x); +} + +double ml_pow(double x, double y) +{ + return pow(x, y); +} diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c new file mode 100644 index 0000000..231cbfd --- /dev/null +++ b/libsensors_iio/software/core/mllite/mpl.c @@ -0,0 +1,72 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+/**
+ * @defgroup MPL mpl
+ * @brief Motion Library - Start Point
+ * Initializes MPL.
+ *
+ * @{
+ * @file mpl.c
+ * @brief MPL start point.
+ */
+
+#include "storage_manager.h"
+#include "log.h"
+#include "mpl.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+#include "mlinclude.h"
+
+/**
+ * @brief Initializes the MPL. Should be called first and once
+ * @return Returns INV_SUCCESS if successful or an error code if not.
+ */
+inv_error_t inv_init_mpl(void)
+{
+ inv_init_storage_manager();
+
+ /* initialize the start callback manager */
+ INV_ERROR_CHECK(inv_init_start_manager());
+
+ /* initialize the data builder */
+ INV_ERROR_CHECK(inv_init_data_builder());
+
+ INV_ERROR_CHECK(inv_enable_results_holder());
+
+ return INV_SUCCESS;
+}
+
+const char ml_ver[] = "InvenSense MPL 5.0.1";
+
+/**
+ * @brief used to get the MPL version.
+ * @param version a string where the MPL version gets stored.
+ * @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_get_version(char **version)
+{
+ INVENSENSE_FUNC_START;
+ /* cast out the const */
+ *version = (char *)&ml_ver;
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief Starts the MPL. Typically called after inv_init_mpl() or after a
+ * inv_stop_mpl() to start the MPL back up an running.
+ * @return INV_SUCCESS if successful or a non-zero error code otherwise.
+ */
+inv_error_t inv_start_mpl(void)
+{
+ INV_ERROR_CHECK(inv_execute_mpl_start_notification());
+ return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
diff --git a/libsensors_iio/software/core/mllite/mpl.h b/libsensors_iio/software/core/mllite/mpl.h new file mode 100644 index 0000000..a6b5ac7 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c new file mode 100644 index 0000000..97ffdec --- /dev/null +++ b/libsensors_iio/software/core/mllite/results_holder.c @@ -0,0 +1,500 @@ +/* + $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 "results_holder.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" +#include "start_manager.h" +#include "data_builder.h" +#include "message_layer.h" + +// 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 + +struct results_t { + long nav_quat[4]; + long gam_quat[4]; + inv_time_t nav_timestamp; + inv_time_t gam_timestamp; + long local_field[3]; /**< local earth's magnetic field */ + long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ + long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ + int acc_state; /**< Describes accel state */ + 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; +}; +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 +*/ +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 +* 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 +* 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; +} + +/** 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 local earth's magnetic field +* @param[in] data Local earth's magnetic field in uT scaled by 2^16. +* Length = 3. Y typically points north, Z typically points down in +* northern hemisphere and up in southern hemisphere. +*/ +void inv_set_local_field(const long *data) +{ + memcpy(rh.local_field, data, sizeof(rh.local_field)); +} + +/** Gets the local earth's magnetic field +* @param[out] data Local earth's magnetic field in uT scaled by 2^16. +* Length = 3. Y typically points north, Z typically points down in +* northern hemisphere and up in southern hemisphere. +*/ +void inv_get_local_field(long *data) +{ + memcpy(data, rh.local_field, sizeof(rh.local_field)); +} + +/** 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) +{ + data[0] = + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); + data[1] = + inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); + data[2] = + (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_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) +{ + memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); + 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) +{ + if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { + inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); + rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); + } + memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); + 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) +{ + long ldata[4]; + inv_error_t result = inv_get_quaternion(ldata); + data[0] = inv_q30_to_float(ldata[0]); + data[1] = inv_q30_to_float(ldata[1]); + data[2] = inv_q30_to_float(ldata[2]); + data[3] = inv_q30_to_float(ldata[3]); + return result; +} + +/** 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] = 1L<<30; + rh.quat_confidence_interval = (float)M_PI; + 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 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; +} + +/** + * @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; + } +} + +/** + * @} + */ diff --git a/libsensors_iio/software/core/mllite/results_holder.h b/libsensors_iio/software/core/mllite/results_holder.h new file mode 100644 index 0000000..a60d7f0 --- /dev/null +++ b/libsensors_iio/software/core/mllite/results_holder.h @@ -0,0 +1,77 @@ +/* + $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 + +void inv_store_gaming_quaternion(const long *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 + +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_6axis_quaternion(long *data); +inv_error_t inv_get_quaternion(long *data); +inv_error_t inv_get_quaternion_float(float *data); +void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp); + +inv_error_t inv_enable_results_holder(); +inv_error_t inv_init_results_holder(void); + +/* Magnetic Field Parameters*/ +void inv_set_local_field(const long *data); +void inv_get_local_field(long *data); +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); + +#ifdef __cplusplus +} +#endif + +#endif // INV_RESULTS_HOLDER_H__ diff --git a/libsensors_iio/software/core/mllite/start_manager.c b/libsensors_iio/software/core/mllite/start_manager.c new file mode 100644 index 0000000..fb758e7 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/start_manager.h b/libsensors_iio/software/core/mllite/start_manager.h new file mode 100644 index 0000000..899e3f5 --- /dev/null +++ b/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/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c new file mode 100644 index 0000000..4b92bfc --- /dev/null +++ b/libsensors_iio/software/core/mllite/storage_manager.c @@ -0,0 +1,200 @@ +/* + $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. + */ +#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 (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) { + checksum = inv_checksum(data, hd->size); + if (checksum == hd->checksum) { + ds.load[entry](data); + } else { + return INV_ERROR_CALIBRATION_LOAD; + } + } + } + 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 (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/libsensors_iio/software/core/mllite/storage_manager.h b/libsensors_iio/software/core/mllite/storage_manager.h new file mode 100644 index 0000000..7255591 --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/accel_auto_cal.h b/libsensors_iio/software/core/mpl/accel_auto_cal.h new file mode 100644 index 0000000..5a53213 --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/adv_func.h b/libsensors_iio/software/core/mpl/adv_func.h new file mode 100644 index 0000000..3f8595f --- /dev/null +++ b/libsensors_iio/software/core/mpl/adv_func.h @@ -0,0 +1,30 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_ADVFUNC_H__ +#define MLDMP_ADVFUNC_H__ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + float inv_compass_angle(const long *compass, const long *grav, const float *quat); + inv_error_t inv_set_dmp_quaternion(long *q); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_ADVFUNC_H__ diff --git a/libsensors_iio/software/core/mpl/authenticate.h b/libsensors_iio/software/core/mpl/authenticate.h new file mode 100644 index 0000000..d7c803b --- /dev/null +++ b/libsensors_iio/software/core/mpl/authenticate.h @@ -0,0 +1,21 @@ +/* + $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); + +#endif /* MLDMP_AUTHENTICATE_H__ */ diff --git a/libsensors_iio/software/core/mpl/auto_calibration.h b/libsensors_iio/software/core/mpl/auto_calibration.h new file mode 100644 index 0000000..3dd9827 --- /dev/null +++ b/libsensors_iio/software/core/mpl/auto_calibration.h @@ -0,0 +1,33 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_IN_USE_AUTO_CALIBRATION_H__ +#define MLDMP_IN_USE_AUTO_CALIBRATION_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_in_use_auto_calibration(void); +inv_error_t inv_disable_in_use_auto_calibration(void); +inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled); +inv_error_t inv_init_in_use_auto_calibration(void); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_IN_USE_AUTO_CALIBRATION_H__ + diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so Binary files differnew file mode 100644 index 0000000..116b618 --- /dev/null +++ b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so diff --git a/libsensors_iio/software/core/mpl/build/android/shared.mk b/libsensors_iio/software/core/mpl/build/android/shared.mk new file mode 100644 index 0000000..79cf9c1 --- /dev/null +++ b/libsensors_iio/software/core/mpl/build/android/shared.mk @@ -0,0 +1,92 @@ +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 + +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 += -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 += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-shared,-Bsymbolic +LFLAGS += $(ANDROID_LINK) +LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## 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) + +$(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/libsensors_iio/software/core/mpl/build/android/static.mk b/libsensors_iio/software/core/mpl/build/android/static.mk new file mode 100644 index 0000000..6ad45de --- /dev/null +++ b/libsensors_iio/software/core/mpl/build/android/static.mk @@ -0,0 +1,110 @@ +MLLITE_LIB_NAME = mllite +LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +OBJFOLDER = $(CURDIR)/obj + +CROSS = arm-none-linux-gnueabi- +COMP= $(CROSS)gcc +LINK= $(CROSS)ar cr + +MLLITE_DIR = $(MLSDK_ROOT)/mllite +MPL_DIR = $(MLSDK_ROOT)/mldmp +MLPLATFORM_DIR = $(MLSDK_ROOT)/platform + +include $(MLSDK_ROOT)/Android-common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += -Wall -fpic +CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT -DLINUX -DANDROID +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(MLPLATFORM_DIR)/include +CFLAGS += -I$(MLSDK_ROOT)/mlutils +CFLAGS += -I$(MLSDK_ROOT)/mlapps/common +CFLAGS += $(MLSDK_INCLUDES) +CFLAGS += $(MLSDK_DEFINES) + +VPATH += $(MLLITE_DIR) +VPATH += $(MLSDK_ROOT)/mlutils +VPATH += $(MLLITE_DIR)/accel +VPATH += $(MLLITE_DIR)/compass +VPATH += $(MLLITE_DIR)/pressure +VPATH += $(MLLITE_DIR)/mlapps/common + +#################################################################################################### +## sources + +ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) + +ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c +ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c +ML_SOURCES += $(MLLITE_DIR)/accel.c +ML_SOURCES += $(MLLITE_DIR)/compass.c +ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c +ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c +ML_SOURCES += $(MLLITE_DIR)/key0_96.c +ML_SOURCES += $(MLLITE_DIR)/pressure.c +ML_SOURCES += $(MLLITE_DIR)/ml.c +ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c +ML_SOURCES += $(MLLITE_DIR)/ml_init.c +ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c +ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c +ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c +ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c +ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c +ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c +ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c +ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c +ML_SOURCES += $(MLLITE_DIR)/mldl.c +ML_SOURCES += $(MLLITE_DIR)/mldmp.c +ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c +ML_SOURCES += $(MLLITE_DIR)/mlstates.c +ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c +ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c +ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c +ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c +ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c +ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c +ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c +ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c +ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c +ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c +ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c +ifeq ($(MPU_NAME),MPU3050) +ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c +else +ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c +endif + +ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) +ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all clean cleanall + +all: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n") + $(LINK) $(LIBRARY) $(ML_OBJS_DST) + $(CROSS)ranlib $(LIBRARY) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/libsensors_iio/software/core/mpl/build/filelist.mk b/libsensors_iio/software/core/mpl/build/filelist.mk new file mode 100644 index 0000000..e18003a --- /dev/null +++ b/libsensors_iio/software/core/mpl/build/filelist.mk @@ -0,0 +1,46 @@ +#### filelist.mk for mpl #### + +# headers only +HEADERS := $(MPL_DIR)/mpu.h + +# 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 +#HEADERS += $(MPL_DIR)/auto_calibration.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 +#SOURCES += $(MPL_DIR)/auto_calibration.c + +INV_SOURCES += $(SOURCES) + +VPATH = $(MPL_DIR) + diff --git a/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h new file mode 100644 index 0000000..4f01fc2 --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/compass_fit.h b/libsensors_iio/software/core/mpl/compass_fit.h new file mode 100644 index 0000000..be3cce8 --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/compass_vec_cal.h b/libsensors_iio/software/core/mpl/compass_vec_cal.h new file mode 100644 index 0000000..a3e044c --- /dev/null +++ b/libsensors_iio/software/core/mpl/compass_vec_cal.h @@ -0,0 +1,34 @@ +/* + $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(); + +#ifdef __cplusplus +} +#endif + +#endif // COMPASS_ONLY_CAL_H__ diff --git a/libsensors_iio/software/core/mpl/fast_no_motion.h b/libsensors_iio/software/core/mpl/fast_no_motion.h new file mode 100644 index 0000000..2a33093 --- /dev/null +++ b/libsensors_iio/software/core/mpl/fast_no_motion.h @@ -0,0 +1,44 @@ +/*
+ $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 N); + inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled); + inv_error_t inv_update_fast_nomot(long *gyro); + + void inv_get_fast_nomot_accel_param(long *cntr, float *param); + void inv_get_fast_nomot_compass_param(long *cntr, float *param); + void inv_set_fast_nomot_accel_threshold(float thresh); + void inv_set_fast_nomot_compass_threshold(float thresh); + void int_set_fast_nomot_gyro_threshold(float thresh); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FAST_NO_MOTION_H__ + diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/libsensors_iio/software/core/mpl/fusion_9axis.h new file mode 100644 index 0000000..616694a --- /dev/null +++ b/libsensors_iio/software/core/mpl/fusion_9axis.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_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); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FUSION9AXIS_H__ diff --git a/libsensors_iio/software/core/mpl/gyro_tc.h b/libsensors_iio/software/core/mpl/gyro_tc.h new file mode 100644 index 0000000..8f1d50e --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/heading_from_gyro.h b/libsensors_iio/software/core/mpl/heading_from_gyro.h new file mode 100644 index 0000000..09ecc42 --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/interpolator.h b/libsensors_iio/software/core/mpl/interpolator.h new file mode 100644 index 0000000..5eb571d --- /dev/null +++ b/libsensors_iio/software/core/mpl/interpolator.h @@ -0,0 +1,103 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef INTERPOLATOR_H +#define INTERPOLATOR_H + +#ifdef __cplusplus +extern "C" { +#endif + +//#include "mltypes.h" + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +#define MAX_INTERPOLATION (20) + +typedef struct { + long x[2]; + long y[4]; +} tInterpolate2; +typedef struct { + long x[2]; + long y[6]; +} tInterpolate3; +typedef struct { + long x[5]; + long y[10]; + int idx1; +} tInterpolate5; +typedef struct { + tInterpolate2 state1; + tInterpolate2 state2; +} tInterpolate4; +typedef struct { + tInterpolate3 state1; + tInterpolate2 state2; +} tInterpolate6; +typedef struct { + tInterpolate2 state1; + tInterpolate4 state2; +} tInterpolate8; +typedef struct { + tInterpolate3 state1; + tInterpolate3 state2; +} tInterpolate9; +typedef struct { + tInterpolate5 state1; + tInterpolate2 state2; +} tInterpolate10; +typedef struct { + tInterpolate4 state1; + tInterpolate3 state2; +} tInterpolate12; +typedef struct { + tInterpolate5 state1; + tInterpolate3 state2; +} tInterpolate15; +typedef struct { + tInterpolate2 state1; + tInterpolate8 state2; +} tInterpolate16; +typedef struct { + tInterpolate9 state1; + tInterpolate2 state2; +} tInterpolate18; +typedef struct { + tInterpolate10 state1; + tInterpolate2 state2; +} tInterpolate20; + +typedef union { + tInterpolate2 u2; + tInterpolate3 u3; + tInterpolate4 u4; + tInterpolate5 u5; + tInterpolate6 u6; + tInterpolate8 u8; + tInterpolate9 u9; + tInterpolate10 u10; + tInterpolate12 u12; + tInterpolate15 u15; + tInterpolate16 u16; + tInterpolate18 u18; + tInterpolate20 u20; +} tInterpolateState; + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ +int inv_get_interp_amount( int x ); +int inv_interpolate( int amount, long input, long *output, tInterpolateState *state ); +long inv_fxmult( long x, long y ); + + +#ifdef __cplusplus +} +#endif + +#endif /* INTERPOLATOR_H */ diff --git a/libsensors_iio/software/core/mpl/inv_log.h b/libsensors_iio/software/core/mpl/inv_log.h new file mode 100644 index 0000000..972844b --- /dev/null +++ b/libsensors_iio/software/core/mpl/inv_log.h @@ -0,0 +1,7 @@ +#include "mltypes.h" +#ifndef INV_INV_LOG_H__ +#define INV_INV_LOG_H__ + +#define INV_LOGE(s) + +#endif // INV_INV_LOG_H__ diff --git a/libsensors_iio/software/core/mpl/inv_math.h b/libsensors_iio/software/core/mpl/inv_math.h new file mode 100644 index 0000000..6620bbf --- /dev/null +++ b/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/libsensors_iio/software/core/mpl/invensense_adv.h b/libsensors_iio/software/core/mpl/invensense_adv.h new file mode 100644 index 0000000..9e59c18 --- /dev/null +++ b/libsensors_iio/software/core/mpl/invensense_adv.h @@ -0,0 +1,30 @@ +/* + $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" diff --git a/libsensors_iio/software/core/mpl/mag_disturb.h b/libsensors_iio/software/core/mpl/mag_disturb.h new file mode 100644 index 0000000..38df919 --- /dev/null +++ b/libsensors_iio/software/core/mpl/mag_disturb.h @@ -0,0 +1,37 @@ +/*
+ $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 + + int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat, + const long *compass, const long *gravity); + + void inv_track_dip_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_disable_dip_tracking(void); + inv_error_t inv_enable_dip_tracking(void); + inv_error_t inv_init_magnetic_disturbance(void);
+ + float Mag3ofNormalizedLong(const long *x);
+
+#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_MAGDISTURB_H__ diff --git a/libsensors_iio/software/core/mpl/mlsetinterrupts.h b/libsensors_iio/software/core/mpl/mlsetinterrupts.h new file mode 100644 index 0000000..a81dabb --- /dev/null +++ b/libsensors_iio/software/core/mpl/mlsetinterrupts.h @@ -0,0 +1,23 @@ +/* + $License: + Copyright (c) 2008 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef MLSETINTERRUPT_H +#define MLSETINTERRUPT_H + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + /* DEPRECATED - Scheduled for removal. Do not use */ + inv_error_t MLSetInterrupts(unsigned short interrupts); + +#ifdef __cplusplus +} +#endif + +#endif /* MLSETINTERRUPT_H */ diff --git a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h new file mode 100644 index 0000000..3779381 --- /dev/null +++ b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h @@ -0,0 +1,57 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id: mlsupervisor_9axis.h 6123 2011-09-30 18:21:11Z mcaramello $ + * + *****************************************************************************/ + +#ifndef MLDMP_MLSUPERVISOR_H__ +#define MLDMP_MLSUPERVISOR_H__ + +#include "mltypes.h" + +//#include "temp_comp.h" + +struct inv_fusion_t { + int compassCount; + long quat[4]; +}; + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_9x_fusion(void); +inv_error_t inv_disable_9x_fusion(void); + +inv_error_t inv_enable_9x_fusion_legacy(void); +inv_error_t inv_disable_9x_fusion_legacy(void); + +inv_error_t inv_enable_9x_fusion_new(void); +inv_error_t inv_disable_9x_fusion_new(void); + +inv_error_t inv_enable_9x_fusion_basic(void); +inv_error_t inv_disable_9x_fusion_basic(void); + +inv_error_t inv_enable_9x_fusion_external(void); +inv_error_t inv_disable_9x_fusion_external(void); + +inv_error_t inv_enable_maintain_heading(void); +inv_error_t inv_disable_maintain_heading(void); + +void inv_set_compass_state(long compassState, long accState, + unsigned long deltaTime, + int magDisturb, int gotBias, + int *new_state, + int *new_accuracy); + +#ifdef __cplusplus +} +#endif + +#endif // MLDMP_MLSUPERVISOR_H__ diff --git a/libsensors_iio/software/core/mpl/motion_no_motion.h b/libsensors_iio/software/core/mpl/motion_no_motion.h new file mode 100644 index 0000000..01cf1c0 --- /dev/null +++ b/libsensors_iio/software/core/mpl/motion_no_motion.h @@ -0,0 +1,28 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */ +#ifndef INV_MOTION_NO_MOTION_H__
+#define INV_MOTION_NO_MOTION_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_motion_no_motion(void);
+inv_error_t inv_disable_motion_no_motion(void);
+inv_error_t inv_init_motion_no_motion(void);
+inv_error_t inv_start_motion_no_motion(void);
+inv_error_t inv_stop_motion_no_motion(void);
+
+inv_error_t inv_set_no_motion_time(long time_ms);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_MOTION_NO_MOTION_H__
diff --git a/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/libsensors_iio/software/core/mpl/no_gyro_fusion.h new file mode 100644 index 0000000..38d5690 --- /dev/null +++ b/libsensors_iio/software/core/mpl/no_gyro_fusion.h @@ -0,0 +1,34 @@ +/*
+ $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); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_NOGYROFUSION_H__ diff --git a/libsensors_iio/software/core/mpl/orientation.h b/libsensors_iio/software/core/mpl/orientation.h new file mode 100644 index 0000000..ab4e45e --- /dev/null +++ b/libsensors_iio/software/core/mpl/orientation.h @@ -0,0 +1,42 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef MLDMP_ORIENTATION_H__ +#define MLDMP_ORIENTATION_H__ + +#include "mltypes.h" +/*******************************************************************************/ +/* Orientations */ +/*******************************************************************************/ + +#define INV_X_UP 0x01 +#define INV_X_DOWN 0x02 +#define INV_Y_UP 0x04 +#define INV_Y_DOWN 0x08 +#define INV_Z_UP 0x10 +#define INV_Z_DOWN 0x20 +#define INV_ORIENTATION_ALL 0x3F + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_orientation(void); + inv_error_t inv_disable_orientation(void); + inv_error_t inv_set_orientation(int orientation); + inv_error_t inv_set_orientation_cb(void (*callback)(unsigned short)); + inv_error_t inv_get_orientation(int *orientation); + inv_error_t inv_get_orientation_state(int * state); + inv_error_t inv_set_orientation_interrupt(unsigned char on); + inv_error_t inv_set_orientation_thresh(float angle, + float hysteresis, + unsigned long time, + unsigned int axis); + +#ifdef __cplusplus +} +#endif + +#endif // MLDMP_ORIENTATION_H__ diff --git a/libsensors_iio/software/core/mpl/progressive_no_motion.h b/libsensors_iio/software/core/mpl/progressive_no_motion.h new file mode 100644 index 0000000..99333e3 --- /dev/null +++ b/libsensors_iio/software/core/mpl/progressive_no_motion.h @@ -0,0 +1,39 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +#ifndef MLDMP_PROG_NO_MOTION_H__ +#define MLDMP_PROG_NO_MOTION_H__ + +#include "mltypes.h" + +#define PROG_NO_MOTION 1 +#define PROG_MOTION 2 + +#ifdef __cplusplus +extern "C" { +#endif + +/* APIs */ +inv_error_t inv_enable_prog_no_motion(void); +inv_error_t inv_disable_prog_no_motion(void); + +/* internal use */ +int inv_get_prog_no_motion_enabled(void); +void inv_get_prog_no_motion_bias_changed(void); +int inv_get_prog_no_motion_state(void); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_PROG_NO_MOTION_H__ diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h new file mode 100644 index 0000000..2cf7a50 --- /dev/null +++ b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h @@ -0,0 +1,70 @@ +/*
+ quat_accuracy_monitor.h
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef QUAT_ACCURARCY_MONITOR_H__
+#define QUAT_ACCURARCY_MONITOR_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+enum accuracy_signal_type_e {
+ TYPE_NAV_QUAT,
+ TYPE_GAM_QUAT,
+ TYPE_NAV_QUAT_ADVANCED,
+ TYPE_GAM_QUAT_ADVANCED,
+ TYPE_NAV_QUAT_BASIC,
+ TYPE_GAM_QUAT_BASIC,
+ TYPE_MAG,
+ TYPE_GYRO,
+ TYPE_ACCEL,
+};
+
+inv_error_t inv_init_quat_accuracy_monitor(void);
+
+void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
+double get_accuracy_threshold(enum accuracy_signal_type_e type);
+void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
+int get_accuracy_weight(enum accuracy_signal_type_e type);
+
+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
+
+void inv_reset_quat_accuracy(void);
+double get_6axis_correction_term(void);
+double get_9axis_correction_term(void);
+int get_9axis_accuracy_state();
+
+void set_6axis_error_average(double value);
+double get_6axis_error_bound(void);
+double get_compass_correction(void);
+double get_9axis_error_bound(void);
+
+float get_confidence_interval(void);
+void set_compass_uncertainty(float value);
+
+inv_error_t inv_enable_quat_accuracy_monitor(void);
+inv_error_t inv_disable_quat_accuracy_monitor(void);
+inv_error_t inv_start_quat_accuracy_monitor(void);
+inv_error_t inv_stop_quat_accuracy_monitor(void);
+
+double get_compassNgravity(void);
+double get_init_compassNgravity(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/libsensors_iio/software/core/mpl/quaternion_supervisor.h new file mode 100644 index 0000000..532e8af --- /dev/null +++ b/libsensors_iio/software/core/mpl/quaternion_supervisor.h @@ -0,0 +1,26 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_QUATERNION_SUPERVISOR_H__
+#define INV_QUATERNION_SUPERVISOR_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_quaternion(void);
+inv_error_t inv_disable_quaternion(void);
+inv_error_t inv_init_quaternion(void);
+inv_error_t inv_start_quaternion(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/libsensors_iio/software/core/mpl/sensor_moments.h b/libsensors_iio/software/core/mpl/sensor_moments.h new file mode 100644 index 0000000..73eb363 --- /dev/null +++ b/libsensors_iio/software/core/mpl/sensor_moments.h @@ -0,0 +1,42 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_SENSOR_MOMENTS_H__ +#define MLDMP_SENSOR_MOMENTS_H__ + +#include "mltypes.h" + +enum moment_ord { + SECOND_ORD=0, + THIRD_ORD, + FOURTH_ORD, + MAX_ORD +}; + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_sm(void);
+ inv_error_t inv_disable_sm(void);
+ inv_error_t inv_sm_record_data(float sample, void *sensor);
+ inv_error_t inv_sm_update_evt_act_state(int motion);
+ void *inv_init_sm(enum moment_ord);
+ float inv_sm_get_filtered_data(void *sensor); + +#ifdef __cplusplus +} +#endif + +#endif // MLDMP_SENSOR_MOMENTS_H__ + diff --git a/libsensors_iio/software/core/mpl/state_storage.h b/libsensors_iio/software/core/mpl/state_storage.h new file mode 100644 index 0000000..c1eb47b --- /dev/null +++ b/libsensors_iio/software/core/mpl/state_storage.h @@ -0,0 +1,25 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_STATE_STORAGE_H__ +#define INV_STATE_STORAGE_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_store_data(const void *data, size_t size, unsigned long module, + unsigned long version); +inv_error_t inv_load_data(void *data, size_t size, unsigned long module, + unsigned long version); + +#ifdef __cplusplus +} +#endif + +#endif // INV_STATE_STORAGE_H__ diff --git a/libsensors_iio/software/simple_apps/common/external_hardware.h b/libsensors_iio/software/simple_apps/common/external_hardware.h new file mode 100644 index 0000000..55e3b20 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/external_hardware.h @@ -0,0 +1,156 @@ +/* + Accelerometer +*/ +#define get_accel_slave_descr NULL + +#ifdef CONFIG_MPU_SENSORS_ADXL34X /* ADI accelerometer */ +struct ext_slave_descr *adxl34x_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr adxl34x_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_BMA150 /* Bosch accelerometer */ +struct ext_slave_descr *bma150_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr bma150_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_BMA222 /* Bosch 222 accelerometer */ +struct ext_slave_descr *bma222_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr bma222_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_BMA250 /* Bosch accelerometer */ +struct ext_slave_descr *bma250_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr bma250_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_KXSD9 /* Kionix accelerometer */ +struct ext_slave_descr *kxsd9_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr kxsd9_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_KXTF9 /* Kionix accelerometer */ +struct ext_slave_descr *kxtf9_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr kxtf9_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_LIS331DLH /* ST accelerometer */ +struct ext_slave_descr *lis331_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr lis331_get_slave_descr +#endif + + +#ifdef CONFIG_MPU_SENSORS_LIS3DH /* ST accelerometer */ +struct ext_slave_descr *lis3dh_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr lis3dh_get_slave_descr +#endif + +/* ST accelerometer in LSM303DLx combo */ +#if defined CONFIG_MPU_SENSORS_LSM303DLX_A +struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr lsm303dlx_a_get_slave_descr +#endif + +/* MPU6050 Accel */ +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 +struct ext_slave_descr *mpu6050_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr mpu6050_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_MMA8450 /* Freescale accelerometer */ +struct ext_slave_descr *mma8450_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr mma8450_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_MMA845X /* Freescale accelerometer */ +struct ext_slave_descr *mma845x_get_slave_descr(void); +#undef get_accel_slave_descr +#define get_accel_slave_descr mma845x_get_slave_descr +#endif + + +/* + Compass +*/ +#define get_compass_slave_descr NULL + +#ifdef CONFIG_MPU_SENSORS_AK8975 /* AKM compass */ +struct ext_slave_descr *ak8975_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr ak8975_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_AMI30X /* AICHI Steel AMI304/305 compass */ +struct ext_slave_descr *ami30x_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr ami30x_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_AMI306 /* AICHI Steel AMI306 compass */ +struct ext_slave_descr *ami306_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr ami306_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_HMC5883 /* Honeywell compass */ +struct ext_slave_descr *hmc5883_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr hmc5883_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_MMC314X /* MEMSIC compass */ +struct ext_slave_descr *mmc314x_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr mmc314x_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_LSM303DLX_M /* ST compass */ +struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr lsm303dlx_m_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_YAS529 /* Yamaha compass */ +struct ext_slave_descr *yas529_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr yas529_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_YAS530 /* Yamaha compass */ +struct ext_slave_descr *yas530_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr yas530_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_HSCDTD002B /* Alps HSCDTD002B compass */ +struct ext_slave_descr *hscdtd002b_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr hscdtd002b_get_slave_descr +#endif + +#ifdef CONFIG_MPU_SENSORS_HSCDTD004A /* Alps HSCDTD004A compass */ +struct ext_slave_descr *hscdtd004a_get_slave_descr(void); +#undef get_compass_slave_descr +#define get_compass_slave_descr hscdtd004a_get_slave_descr +#endif +/* + Pressure +*/ +#define get_pressure_slave_descr NULL + +#ifdef CONFIG_MPU_SENSORS_BMA085 /* BMA pressure */ +struct ext_slave_descr *bma085_get_slave_descr(void); +#undef get_pressure_slave_descr +#define get_pressure_slave_descr bma085_get_slave_descr +#endif diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.c b/libsensors_iio/software/simple_apps/common/fopenCMake.c new file mode 100644 index 0000000..2936109 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/fopenCMake.c @@ -0,0 +1,56 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/****************************************************************************** + * + * $Id: fopenCMake.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +#include <string.h> + +#include "fopenCMake.h" +#include "path_configure.h" + +/** + * @brief Replacement for fopen that concatenates the location of the + * source tree onto the filename path. + * It looks in 3 locations: + * - in the current directory, + * - then it looks in "..", + * - lastly in the define UNITTEST_SOURCE_DIR which + * gets defined by CMake. + * @param filename + * Filename relative to base of source directory. + * @param prop + * Second argument to fopen. + */ +FILE *fopenCMake(const char *filename, const char *prop) +{ + char path[150]; + FILE *file; + + // Look first in current directory + file = fopen(filename, prop); + if (file == NULL) { + // Now look in ".." +#ifdef WIN32 + strcpy(path, "..\\"); +#else + strcpy(path, "../"); +#endif + strcat(path, filename); + file = fopen(path, prop); + if (file == NULL) { + // Now look in definition by CMake + strcpy(path, PATH_SOURCE_DIR); + strcat(path, filename); + file = fopen(path, prop); + } + } + return file; +} + + diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.h b/libsensors_iio/software/simple_apps/common/fopenCMake.h new file mode 100644 index 0000000..c5eba39 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/fopenCMake.h @@ -0,0 +1,21 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef FOPEN_CMAKE_H__ +#define FOPEN_CMAKE_H__ + +#include <stdio.h> + +#ifdef __cplusplus +extern "C" { +#endif + +FILE *fopenCMake( const char *filename, const char *prop ); + +#ifdef __cplusplus +} +#endif + +#endif // FOPEN_CMAKE_H__ diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.c b/libsensors_iio/software/simple_apps/common/gestureMenu.c new file mode 100644 index 0000000..2a9487c --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/gestureMenu.c @@ -0,0 +1,725 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $ + *****************************************************************************/ + +#define _USE_MATH_DEFINES +#include <stdio.h> +#include <stddef.h> +#include <math.h> +#include <string.h> + +#include "ml.h" +#include "mlmath.h" +#include "gesture.h" +#include "orientation.h" +#include "gestureMenu.h" +#include "fifo.h" + +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "gest" +#include "log.h" +#include "mldl_cfg.h" + +static unsigned long sensors[] = { + INV_NINE_AXIS, + INV_THREE_AXIS_GYRO, + INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL, + INV_THREE_AXIS_ACCEL, + INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS, + INV_THREE_AXIS_COMPASS, + INV_SIX_AXIS_GYRO_ACCEL, + INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS, + INV_SIX_AXIS_ACCEL_COMPASS, +}; + +static char *sensors_string[] = { + "INV_NINE_AXIS", + "INV_THREE_AXIS_GYRO", + "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL", + "INV_THREE_AXIS_ACCEL", + "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS", + "INV_THREE_AXIS_COMPASS", + "INV_SIX_AXIS_GYRO_ACCEL", + "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS", + "INV_SIX_AXIS_ACCEL_COMPASS", +}; + +/** + * Prints the menu with the current thresholds + * + * @param params The parameters to print + */ +void PrintGestureMenu(tGestureMenuParams const * const params) +{ + MPL_LOGI("Press h at any time to re-display this menu\n"); + MPL_LOGI("TAP PARAMETERS:\n"); + MPL_LOGI(" Use LEFT and RIGHT arrows to adjust Tap Time \n\n"); + MPL_LOGI(" j : Increase X threshold : %5d\n", + params->xTapThreshold); + MPL_LOGI(" J (Shift-j): Decrease X threshold\n"); + MPL_LOGI(" k : Increase Y threshold : %5d\n", + params->yTapThreshold); + MPL_LOGI(" K (Shift-k): Decrease Y threshold\n"); + MPL_LOGI(" i : Increase Z threshold : %5d\n", + params->zTapThreshold); + MPL_LOGI(" I (Shift-i): Decrease Z threshold\n"); + MPL_LOGI(" l : Increase tap time : %5d\n", + params->tapTime); + MPL_LOGI(" L (Shift-l): Decrease tap time\n"); + MPL_LOGI(" o : Increase next tap time : %5d\n", + params->nextTapTime); + MPL_LOGI(" O (Shift-o): Increase next tap time\n"); + MPL_LOGI(" u : Increase max Taps : %5d\n", + params->maxTaps); + MPL_LOGI(" U (Shift-u): Increase max Taps\n"); + + MPL_LOGI("SHAKE PARAMETERS:\n"); + MPL_LOGI(" x : Increase X threshold : %5d\n", + params->xShakeThresh); + MPL_LOGI(" X (Shift-x): Decrease X threshold\n"); + MPL_LOGI(" y : Increase Y threshold : %5d\n", + params->yShakeThresh); + MPL_LOGI(" Y (Shift-y): Decrease Y threshold\n"); + MPL_LOGI(" z : Increase Z threshold : %5d\n", + params->zShakeThresh); + MPL_LOGI(" Z (Shift-z): Decrease Z threshold\n"); + MPL_LOGI(" s : Toggle Shake Function : %5d\n", + params->shakeFunction); + MPL_LOGI(" t : Increase Shake Time : %5d\n", + params->shakeTime); + MPL_LOGI(" T (Shift-T): Decrease Shake Time\n"); + MPL_LOGI(" n : Increase Next Shake Time : %5d\n", + params->nextShakeTime); + MPL_LOGI(" N (Shift-n): Decrease Next Shake Time\n"); + MPL_LOGI(" m : Increase max Shakes : %5d\n", + params->maxShakes); + MPL_LOGI(" M (Shift-m): Decrease max Shakes\n"); + MPL_LOGI("SNAP PARAMETERS:\n"); + MPL_LOGI(" p : Increase Pitch threshold : %5d\n", + params->xSnapThresh); + MPL_LOGI(" P (Shift-p): Decrease Pitch threshold\n"); + MPL_LOGI(" r : Increase Roll threshold : %5d\n", + params->ySnapThresh); + MPL_LOGI(" R (Shift-r): Decrease Roll threshold\n"); + MPL_LOGI(" a : Increase yAw threshold : %5d\n", + params->zSnapThresh); + MPL_LOGI(" A (Shift-a): Decrease yAw threshold\n"); + MPL_LOGI("YAW ROTATION PARAMETERS:\n"); + MPL_LOGI(" e : Increase yaW Rotate time : %5d\n", + params->yawRotateTime); + MPL_LOGI(" E (Shift-r): Decrease yaW Rotate time\n"); + MPL_LOGI(" w : Increase yaW Rotate threshold : %5d\n", + params->yawRotateThreshold); + MPL_LOGI(" W (Shift-w): Decrease yaW Rotate threshold\n"); + MPL_LOGI("ORIENTATION PARAMETER:\n"); + MPL_LOGI(" d : Increase orientation angle threshold : %5f\n", + params->orientationThreshold); + MPL_LOGI(" D (Shift-d): Decrease orientation angle threshold\n"); + MPL_LOGI("FIFO RATE:\n"); + MPL_LOGI(" f : Increase fifo divider : %5d\n", + inv_get_fifo_rate()); + MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); + MPL_LOGI("REQUESTED SENSORS:\n"); + MPL_LOGI(" S (Shift-s): Toggle in use sensors : %s\n", + sensors_string[params->sensorsIndex]); + MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); + + /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */ + /* S is available */ + + MPL_LOGI("\n\n"); +} + +/** + * Handles a keyboard input and updates an appropriate threshold, prints then + * menu or returns false if the character is not processed. + * + * @param params The parameters to modify if the thresholds are updated + * @param ch The input character + * + * @return true if the character was processed, false otherwise + */ +inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch) +{ + int result = INV_SUCCESS; + /* Dynamic keyboard processing */ + + switch (ch) { + case 'j': + params->xTapThreshold += 20; + // Intentionally fall through + case 'J': { + params->xTapThreshold -= 10; + if (params->xTapThreshold < 0) params->xTapThreshold = 0; + result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetTapThresh returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n", + params->xTapThreshold); + } break; + case 'k': + params->yTapThreshold += 20; + // Intentionally fall through + case 'K': { + params->yTapThreshold -= 10; + if (params->yTapThreshold < 0) params->yTapThreshold = 0; + result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetTapThresh returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n", + params->yTapThreshold); + } break; + case 'i': + params->zTapThreshold += 20; + // Intentionally fall through + case 'I': { + params->zTapThreshold -= 10; + if (params->zTapThreshold < 0) params->zTapThreshold = 0; + result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetTapThresh returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n", + params->zTapThreshold); + } break; + + case 'l': + params->tapTime += 20; + // Intentionally fall through + case 'L': { + params->tapTime -= 10; + if (params->tapTime < 0) params->tapTime = 0; + result = inv_set_next_tap_time(params->tapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); + } + MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime); + } break; + case 'o': + params->nextTapTime += 20; + // Intentionally fall through + case 'O': { + params->nextTapTime -= 10; + if (params->nextTapTime < 0) params->nextTapTime = 0; + result = MLSetNextTapTime(params->nextTapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetNextTapTime returned :%d\n", result); + } + MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime); + } break; + case 'u': + params->maxTaps += 2; + // Intentionally fall through + case 'U': { + params->maxTaps -= 1; + if (params->maxTaps < 0) params->maxTaps = 0; + result = inv_set_max_taps(params->maxTaps); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_taps returned :%d\n", result); + } + MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps); + } break; + case 's': { + int shakeParam; + params->shakeFunction = (params->shakeFunction + 1) % 2; + switch (params->shakeFunction) + { + case 0: + shakeParam = INV_NO_RETRACTION; + MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n"); + break; + case 1: + shakeParam = INV_RETRACTION; + MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n"); + break; + }; + result = inv_set_shake_func(shakeParam); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_func returned :%d\n", result); + } + } break; + case 'x': + params->xShakeThresh += 200; + // Intentionally fall through + case 'X': { + params->xShakeThresh -= 100; + result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh); + } break; + case 'y': + params->yShakeThresh += 200; + // Intentionally fall through + case 'Y': { + params->yShakeThresh -= 100; + result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh); + } break; + case 'z': + params->zShakeThresh += 200; + // Intentionally fall through + case 'Z':{ + params->zShakeThresh -= 100; + result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh); + } break; + case 'r': + params->ySnapThresh += 20; + // Intentionally fall through + case 'R': { + params->ySnapThresh -= 10; + result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh); + } break; + case 'p': + params->xSnapThresh += 20; + // Intentionally fall through + case 'P': { + params->xSnapThresh -= 10; + result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n", + params->xSnapThresh); + } break; + case 'a': + params->zSnapThresh += 20; + case 'A': { + params->zSnapThresh -= 10; + result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh); + } break; + + case 't': + params->shakeTime += 20; + case 'T':{ + params->shakeTime -= 10; + result = inv_set_shake_time(params->shakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_time returned :%d\n", result); + } + MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime); + } break; + case 'n': + params->nextShakeTime += 20; + case 'N':{ + params->nextShakeTime -= 10; + result = inv_set_next_shake_time(params->nextShakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); + } + MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime); + } break; + case 'm': + params->maxShakes += 2; + case 'M':{ + params->maxShakes -= 1; + result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_shakes returned :%d\n", result); + } + MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes); + } break; + case 'e': + params->yawRotateTime += 20; + case 'E':{ + params->yawRotateTime -= 10; + result = inv_set_yaw_rotate_time(params->yawRotateTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); + } + MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime); + } break; + case 'w': + params->yawRotateThreshold += 2; + case 'W':{ + params->yawRotateThreshold -= 1; + result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold); + } break; + case 'c': + params->shakeRejectValue += 0.20f; + case 'C':{ + params->shakeRejectValue -= 0.10f; + result = inv_set_tap_shake_reject(params->shakeRejectValue); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); + } + MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue); + } break; + case 'd': + params->orientationThreshold += 10; + case 'D':{ + params->orientationThreshold -= 5; + if (params->orientationThreshold > 90) { + params->orientationThreshold = 90; + } + + if (params->orientationThreshold < 0 ) { + params->orientationThreshold = 0; + } + + result = inv_set_orientation_thresh(params->orientationThreshold, + 5, 80, + INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result); + } + MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d," + " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n", + params->orientationThreshold, 5, 80); + } break; + case 'f': + result = inv_set_fifo_rate(inv_get_fifo_rate() + 1); + MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); + break; + case 'F': + { + unsigned short newRate = inv_get_fifo_rate(); + if (newRate > 0) + newRate--; + result = inv_set_fifo_rate(newRate); + MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); + break; + } + case 'S': + params->sensorsIndex++; + if (params->sensorsIndex >= ARRAY_SIZE(sensors)) { + params->sensorsIndex = 0; + } + result = inv_set_mpu_sensors( + sensors[params->sensorsIndex] & params->available_sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result, + sensors_string[params->sensorsIndex]); + break; + case 'h': + case 'H': { + PrintGestureMenu(params); + } break; + default: { + result = INV_ERROR; + } break; + }; + return result; +} + +/** + * Initializes the tGestureMenuParams to a set of defaults. + * + * @param params The parameters to initialize. + */ +void GestureMenuSetDefaults(tGestureMenuParams * const params) +{ + params->xTapThreshold = 100; + params->yTapThreshold = 100; + params->zTapThreshold = 100; + params->tapTime = 100; + params->nextTapTime = 600; + params->maxTaps = 2; + params->shakeRejectValue = 0.8f; + params->xShakeThresh = 750; + params->yShakeThresh = 750; + params->zShakeThresh = 750; + params->xSnapThresh = 160; + params->ySnapThresh = 320; + params->zSnapThresh = 160; + params->shakeTime = 100; + params->nextShakeTime = 1000; + params->shakeFunction = 0; + params->maxShakes = 3; + params->yawRotateTime = 80; + params->yawRotateThreshold = 70; + params->orientationThreshold = 60; + params->sensorsIndex = 0; + params->available_sensors = INV_NINE_AXIS; +} + +void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, + unsigned long available_sensors) +{ + params->available_sensors = available_sensors; +} +/** + * Call the appropriate MPL set threshold functions and checkes the error codes + * + * @param params The parametrs to use in setting the thresholds + * + * @return INV_SUCCESS or the first error code encountered. + */ +inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params) +{ + inv_error_t result = INV_SUCCESS; + + result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); + return result; + } + result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); + return result; + } + result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); + return result; + } + result = inv_set_next_tap_time(params->tapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); + return result; + } + result = MLSetNextTapTime(params->nextTapTime); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetNextTapTime returned :%d\n", result); + return result; + } + result = inv_set_max_taps(params->maxTaps); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_taps returned :%d\n", result); + return result; + } + result = inv_set_tap_shake_reject(params->shakeRejectValue); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); + return result; + } + + //Set up shake gesture + result = inv_set_shake_func(params->shakeFunction); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_func returned :%d\n", result); + return result; + } + result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + return result; + } + result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + return result; + } + result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); + return result; + } + result = inv_set_shake_time(params->shakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_shake_time returned :%d\n", result); + return result; + } + result = inv_set_next_shake_time(params->nextShakeTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); + return result; + } + result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_max_shakes returned :%d\n", result); + return result; + } + + // Yaw rotate settings + result = inv_set_yaw_rotate_time(params->yawRotateTime); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); + return result; + } + result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); + return result; + } + + // Orientation settings + result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80, + INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result); + return result; + } + + // Requested Sensors + result = inv_set_mpu_sensors( + sensors[params->sensorsIndex] & params->available_sensors); + if (INV_SUCCESS != result) { + MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result, + sensors[params->sensorsIndex] & params->available_sensors); + return result; + } + + return INV_SUCCESS; +} + +void PrintGesture(tGesture* gesture) +{ + float speed; + char type[1024]; + switch (gesture->type) + { + case INV_TAP: + { + if (gesture->meta < 0) { + snprintf(type,sizeof(type),"-"); + } else { + snprintf(type,sizeof(type),"+"); + } + + switch (ABS(gesture->meta)) + { + case 1: + strcat(type,"X"); + break; + case 2: + strcat(type,"Y"); + break; + case 3: + strcat(type,"Z"); + break; + default: + strcat(type,"ERROR"); + break; + }; + MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n", + type, + gesture->num, + gesture->strength, + gesture->speed, + gesture->reserved, + (180 / M_PI) * atan2( + (float)gesture->strength, (float)gesture->speed), + (180 / M_PI) * atan2( + (float)gesture->speed, (float)gesture->reserved), + (180 / M_PI) * atan2( + (float)gesture->strength, (float)gesture->reserved) + ); + } + break; + case INV_ROLL_SHAKE: + case INV_PITCH_SHAKE: + case INV_YAW_SHAKE: + { + if (gesture->strength){ + snprintf(type, sizeof(type), "Snap : "); + } else { + snprintf(type, sizeof(type), "Shake: "); + } + + if (gesture->meta==0) { + strcat(type, "+"); + } else { + strcat(type, "-"); + } + + if (gesture->type == INV_ROLL_SHAKE) { + strcat(type, "Roll "); + } else if (gesture->type == INV_PITCH_SHAKE) { + strcat(type, "Pitch "); + } else if (gesture->type == INV_YAW_SHAKE) { + strcat(type, "Yaw "); + } + + speed = (float)gesture->speed + + (float)(gesture->reserved / (float)(1 << 16)); + MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed); + } + break; + case INV_YAW_IMAGE_ROTATE: + { + if (gesture->meta == 0) { + snprintf(type, sizeof(type), "Positive "); + } else { + snprintf(type, sizeof(type), "Negative "); + } + MPL_LOGI("%s Yaw Image Rotation\n", type); + } + break; + default: + MPL_LOGE("Unknown Gesture received\n"); + break; + } +} + +/** + * Prints the new or current orientation using MPL_LOGI and remembers the last + * orientation to print orientation flips. + * + * @param orientation the new or current orientation. 0 to reset. + */ +void PrintOrientation(unsigned short orientation) +{ + // Determine if it was a flip + static int sLastOrientation = 0; + int flip = orientation | sLastOrientation; + + if ((INV_X_UP | INV_X_DOWN) == flip) { + MPL_LOGI("Flip about the X Axis: \n"); + } else if ((INV_Y_UP | INV_Y_DOWN) == flip) { + MPL_LOGI("Flip about the Y axis: \n"); + } else if ((INV_Z_UP | INV_Z_DOWN) == flip) { + MPL_LOGI("Flip about the Z axis: \n"); + } + sLastOrientation = orientation; + + switch (orientation) { + case INV_X_UP: + MPL_LOGI("X Axis is up\n"); + break; + case INV_X_DOWN: + MPL_LOGI("X Axis is down\n"); + break; + case INV_Y_UP: + MPL_LOGI("Y Axis is up\n"); + break; + case INV_Y_DOWN: + MPL_LOGI("Y Axis is down\n"); + break; + case INV_Z_UP: + MPL_LOGI("Z Axis is up\n"); + break; + case INV_Z_DOWN: + MPL_LOGI("Z Axis is down\n"); + break; + case 0: + break; /* Not an error. Resets sLastOrientation */ + default: + MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation); + break; + } +} + + diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.h b/libsensors_iio/software/simple_apps/common/gestureMenu.h new file mode 100644 index 0000000..8f804e1 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/gestureMenu.h @@ -0,0 +1,75 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/***************************************************************************** * + * $Id: gestureMenu.h 5705 2011-06-28 19:32:29Z nroyer $ + ******************************************************************************/ +/** + * @defgroup + * @brief + * + * @{ + * @file gestureMenu.h + * @brief + * + * + */ + +#ifndef __GESTUREMENU_H__ +#define __GESTUREMENU_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************/ + typedef struct sGestureMenuParams { + /* Tap Params */ + int xTapThreshold; + int yTapThreshold; + int zTapThreshold; + int tapTime; + int nextTapTime; + int maxTaps; + float shakeRejectValue; + + /* Shake Params */ + int xShakeThresh; + int yShakeThresh; + int zShakeThresh; + int xSnapThresh; + int ySnapThresh; + int zSnapThresh; + int shakeTime; + int nextShakeTime; + int shakeFunction; + int maxShakes; + + /* Yaw rotate params */ + int yawRotateTime; + int yawRotateThreshold; + + /* Orientation */ + float orientationThreshold; + int sensorsIndex; + unsigned long available_sensors; + } tGestureMenuParams; + + void PrintGestureMenu(tGestureMenuParams const * const params) ; + inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params,char ch); + void PrintGesture(gesture_t* gesture); + void PrintOrientation(unsigned short orientation); + void GestureMenuSetDefaults(tGestureMenuParams * const params); + void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, + unsigned long available_sensors); + inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params); + +/******************************************************************************/ + +#ifdef __cplusplus +} +#endif + +#endif // __GESTUREMENU_H__ diff --git a/libsensors_iio/software/simple_apps/common/helper.c b/libsensors_iio/software/simple_apps/common/helper.c new file mode 100644 index 0000000..4d634bd --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/helper.c @@ -0,0 +1,110 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: helper.c 4367 2010-12-21 03:02:55Z prao $ + * + *******************************************************************************/ + +#include <stdio.h> +#ifdef _WIN32 +#include <windows.h> +#include <conio.h> +#endif +#ifdef LINUX +#include <sys/select.h> +#endif +#include <time.h> +#include <string.h> + +#include "ml.h" +#include "slave.h" +#include "mldl.h" +#include "mltypes.h" +#include "mlstates.h" +#include "compass.h" + +#include "mlsl.h" +#include "ml.h" + +#include "helper.h" +#include "mlsetup.h" +#include "fopenCMake.h" +#include "int.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-helper" + +#ifdef AIO +extern inv_error_t MLSLSetYamahaCompassDataMode(unsigned char mode); +#endif + +// Keyboard hit function +int ConsoleKbhit(void) +{ +#ifdef _WIN32 + return _kbhit(); +#else + struct timeval tv; + fd_set read_fd; + + tv.tv_sec=0; + tv.tv_usec=0; + FD_ZERO(&read_fd); + FD_SET(0,&read_fd); + + if(select(1, &read_fd, NULL, NULL, &tv) == -1) + return 0; + + if(FD_ISSET(0,&read_fd)) + return 1; + + return 0; +#endif +} + +char ConsoleGetChar(void) { +#ifdef _WIN32 + return _getch(); +#else + return getchar(); +#endif +} +struct mpuirq_data** InterruptPoll(int *handles, int numHandles, long tv_sec, long tv_usec) +{ + struct mpuirq_data **data; + void *tmp; + int ii; + const int irq_data_size = sizeof(**data) * numHandles + + sizeof(*data) * numHandles; + + tmp = (void *)inv_malloc(irq_data_size); + memset(tmp, 0, irq_data_size); + data = (struct mpuirq_data **)tmp; + for (ii = 0; ii < numHandles; ii++) { + data[ii] = (struct mpuirq_data *)((unsigned long)tmp + + (sizeof(*data) * numHandles) + sizeof(**data) * ii); + } + + if (IntProcess(handles, numHandles, data, tv_sec, tv_usec) > 0) { + for (ii = 0; ii < numHandles; ii++) { + if (data[ii]->interruptcount) { + inv_interrupt_handler(ii); + } + } + } + + /* Return data incase the application needs to look at the timestamp or + other part of the data */ + return data; +} + +void InterruptPollDone(struct mpuirq_data ** data) +{ + inv_free(data); +} diff --git a/libsensors_iio/software/simple_apps/common/helper.h b/libsensors_iio/software/simple_apps/common/helper.h new file mode 100644 index 0000000..b2da520 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/helper.h @@ -0,0 +1,103 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: helper-customer.h 5770 2011-07-14 01:34:10Z mcaramello $ + * + *******************************************************************************/ + +#ifndef HELPER_C_H +#define HELPER_C_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "mlerrorcode.h" + +/* + Defines +*/ + +#define CALL_N_CHECK(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + } \ +} + +#define CALL_CHECK_N_RETURN_ERROR(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return r35uLt; \ + } \ +} + +// for functions returning void +#define CALL_CHECK_N_RETURN(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return; \ + } \ +} + +#define CALL_CHECK_N_EXIT(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + exit (r35uLt); \ + } \ +} + + +#define CALL_CHECK_N_CALLBACK(f, cb) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + cb; \ + } \ +} + +#define CALL_CHECK_N_GOTO(f, label) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + goto label; \ + } \ +} + +#define DEFAULT_PLATFORM PLATFORM_ID_MSB_V2 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 +#define DEFAULT_COMPASS_ID COMPASS_ID_AK8975 + +#define DataLogger(x) NULL +#define DataLoggerSelector(x) // +#define DataLoggerCb(x) NULL +#define findComm() (9) +#define MenuHwChoice(p,a,c) (*p = DEFAULT_PLATFORM, *a = DEFAULT_ACCEL_ID, \ + *c = DEFAULT_COMPASS_ID, INV_ERROR) + + char ConsoleGetChar(void); + int ConsoleKbhit(void); + struct mpuirq_data **InterruptPoll( + int *handles, int numHandles, long tv_sec, long tv_usec); + void InterruptPollDone(struct mpuirq_data ** data); + +#ifdef __cplusplus +} +#endif + +#endif // HELPER_C_H diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/libsensors_iio/software/simple_apps/common/mlerrorcode.c new file mode 100644 index 0000000..25b0df6 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.c @@ -0,0 +1,96 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +#include <stdio.h> +#include <string.h> + +#include "mltypes.h" +#include "mlerrorcode.h" + +#define ERROR_CODE_CASE(CODE) \ + case CODE: \ + return #CODE \ + +/** + * @brief return a string containing the label assigned to the error code. + * + * @param errorcode + * The errorcode value of which the label has to be returned. + * + * @return A string containing the error code label. + */ +char* MLErrorCode(inv_error_t errorcode) +{ + switch(errorcode) { + ERROR_CODE_CASE(INV_SUCCESS); + ERROR_CODE_CASE(INV_ERROR); + ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER); + ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED); + ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED); + ERROR_CODE_CASE(INV_ERROR_DMP_STARTED); + ERROR_CODE_CASE(INV_ERROR_NOT_OPENED); + ERROR_CODE_CASE(INV_ERROR_OPENED); + ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE); + ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED); + ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO); + ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE); + ERROR_CODE_CASE(INV_ERROR_FILE_OPEN); + ERROR_CODE_CASE(INV_ERROR_FILE_READ); + ERROR_CODE_CASE(INV_ERROR_FILE_WRITE); + + ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED); + ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR); + ERROR_CODE_CASE(INV_ERROR_SERIAL_READ); + ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE); + ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED); + + ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION); + ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE); + + ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW); + ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER); + ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT); + ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA); + ERROR_CODE_CASE(INV_ERROR_MEMORY_SET); + + ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR); + ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR); + + ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR); + ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE); + ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED); + ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED); + + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW); + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW); + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY); + ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR); + + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD); + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE); + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN); + ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM); + + default: + { + #define UNKNOWN_ERROR_CODE 1234 + return ERROR_NAME(UNKNOWN_ERROR_CODE); + break; + } + + } +} + +/** + * @} + */ diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/libsensors_iio/software/simple_apps/common/mlerrorcode.h new file mode 100644 index 0000000..9a35792 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlerrorcode.h @@ -0,0 +1,86 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $ + * + *******************************************************************************/ + +#ifndef _MLERRORCODE_H_ +#define _MLERRORCODE_H_ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Defines +*/ +#define CALL_N_CHECK(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + } \ +} + +#define CALL_CHECK_N_RETURN_ERROR(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return r35uLt; \ + } \ +} + +// for functions returning void +#define CALL_CHECK_N_RETURN(f) do { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + return; \ + } \ + } while(0) + +#define CALL_CHECK_N_EXIT(f) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + exit (r35uLt); \ + } \ +} + + +#define CALL_CHECK_N_CALLBACK(f, cb) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + cb; \ + } \ +} + +#define CALL_CHECK_N_GOTO(f, label) { \ + unsigned int r35uLt = f; \ + if(INV_SUCCESS != r35uLt) { \ + MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ + __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ + goto label; \ + } \ +} + +char* MLErrorCode(inv_error_t errorcode); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c new file mode 100644 index 0000000..f11bce9 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlsetup.c @@ -0,0 +1,1722 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/****************************************************************************** + * + * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $ + * + *****************************************************************************/ +#undef MPL_LOG_NDEBUG +#ifdef UNITTESTING +#define MPL_LOG_NDEBUG 1 +#else +#define MPL_LOG_NDEBUG 0 +#endif + +/** + * @defgroup MLSETUP + * @brief The Motion Library external slaves setup override suite. + * + * Use these APIs to override the kernel/default settings in the + * corresponding data structures for gyros, accel, and compass. + * + * @{ + * @file mlsetup.c + * @brief The Motion Library external slaves setup override suite. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +/* + Defines +*/ +/* these have to appear before inclusion of mpu.h */ +#define CONFIG_MPU_SENSORS_KXSD9 y // Kionix accel +#define CONFIG_MPU_SENSORS_KXTF9 y // Kionix accel +#define CONFIG_MPU_SENSORS_LIS331DLH y // ST accelerometer +#define CONFIG_MPU_SENSORS_LSM303DLX_A y // ST accelerometer in LSM303DLx combo +#define CONFIG_MPU_SENSORS_LIS3DH y // ST accelerometer +#define CONFIG_MPU_SENSORS_BMA150 y // Bosch 150 accelerometer +#define CONFIG_MPU_SENSORS_BMA222 y // Bosch 222 accelerometer +#define CONFIG_MPU_SENSORS_BMA250 y // Bosch 250 accelerometer +#define CONFIG_MPU_SENSORS_ADXL34X y // AD 345 or 346 accelerometer +#define CONFIG_MPU_SENSORS_MMA8450 y // Freescale MMA8450 accelerometer +#define CONFIG_MPU_SENSORS_MMA845X y // Freescale MMA845X accelerometer +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 +#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y // Invensense MPU6050 built-in accelerometer +#endif + +#define CONFIG_MPU_SENSORS_AK8975 y // AKM compass +#define CONFIG_MPU_SENSORS_AMI30X y // AICHI AMI304/305 compass +#define CONFIG_MPU_SENSORS_AMI306 y // AICHI AMI306 compass +#define CONFIG_MPU_SENSORS_HMC5883 y // Honeywell compass +#define CONFIG_MPU_SENSORS_LSM303DLX_M y // ST compass in LSM303DLx combo +#define CONFIG_MPU_SENSORS_YAS529 y // Yamaha compass +#define CONFIG_MPU_SENSORS_YAS530 y // Yamaha compass +#define CONFIG_MPU_SENSORS_MMC314X y // MEMSIC compass +#define CONFIG_MPU_SENSORS_HSCDTD002B y // ALPS compass +#define CONFIG_MPU_SENSORS_HSCDTD004A y // ALPS HSCDTD004A compass + +#define CONFIG_MPU_SENSORS_BMA085 y // Bosch 085 pressure + +#include "external_hardware.h" + +#include <stdio.h> +#include <string.h> + +#include "slave.h" +#include "compass.h" +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mlsetup" + +#include "linux/mpu.h" + +#include "mlsetup.h" + +#ifdef LINUX +#include "errno.h" +#endif + +/* Override these structures from mldl.c */ +extern struct ext_slave_descr g_slave_accel; +extern struct ext_slave_descr g_slave_compass; +//extern struct ext_slave_descr g_slave_pressure; +/* Platform Data */ +//extern struct mpu_platform_data g_pdata; +extern struct ext_slave_platform_data g_pdata_slave_accel; +extern struct ext_slave_platform_data g_pdata_slave_compass; +//extern struct ext_slave_platform_data g_pdata_slave_pressure; +signed char g_gyro_orientation[9]; + +/* + Typedefs +*/ +typedef void tSetupFuncAccel(void); +typedef void tSetupFuncCompass(void); +typedef void tSetupFuncPressure(void); + +#ifdef LINUX +#include <sys/ioctl.h> +#endif + +/********************************************************************* + Dragon - PLATFORM_ID_DRAGON_PROTOTYPE +*********************************************************************/ +/** + * @internal + * @brief performs a 180' rotation around Z axis to reflect + * usage of the multi sensor board (MSB) with the + * beagleboard + * @note assumes well formed mounting matrix, with only + * one 1 for each row. + */ +static void Rotate180DegAroundZAxis(signed char matrix[]) +{ + int ii; + for(ii=0; ii<6; ii++) { + matrix[ii] = -matrix[ii]; + } +} + +/** + * @internal + * Sets the orientation based on the position of the mounting. For different + * devices the relative position to pin 1 will be different. + * + * Positions are: + * - 0-3 are Z up + * - 4-7 are Z down + * - 8-11 are Z right + * - 12-15 are Z left + * - 16-19 are Z front + * - 20-23 are Z back + * + * @param position The position of the orientation + * @param orientation the location to store the new oreintation + */ +static inv_error_t SetupOrientation(unsigned int position, + signed char *orientation) +{ + memset(orientation, 0, 9); + switch (position){ + case 0: + /*-------------------------*/ + orientation[0] = +1; + orientation[4] = +1; + orientation[8] = +1; + break; + case 1: + /*-------------------------*/ + orientation[1] = +1; + orientation[3] = -1; + orientation[8] = +1; + break; + case 2: + /*-------------------------*/ + orientation[0] = -1; + orientation[4] = -1; + orientation[8] = +1; + break; + case 3: + /*-------------------------*/ + orientation[1] = -1; + orientation[3] = +1; + orientation[8] = +1; + break; + case 4: + /*-------------------------*/ + orientation[0] = -1; + orientation[4] = +1; + orientation[8] = -1; + break; + case 5: + /*-------------------------*/ + orientation[1] = -1; + orientation[3] = -1; + orientation[8] = -1; + break; + case 6: + /*-------------------------*/ + orientation[0] = +1; + orientation[4] = -1; + orientation[8] = -1; + break; + case 7: + /*-------------------------*/ + orientation[1] = +1; + orientation[3] = +1; + orientation[8] = -1; + break; + case 8: + /*-------------------------*/ + orientation[2] = +1; + orientation[3] = +1; + orientation[7] = +1; + break; + case 9: + /*-------------------------*/ + orientation[2] = +1; + orientation[4] = +1; + orientation[6] = -1; + break; + case 10: + orientation[2] = +1; + orientation[3] = -1; + orientation[7] = -1; + break; + case 11: + orientation[2] = +1; + orientation[4] = -1; + orientation[6] = +1; + break; + case 12: + orientation[2] = -1; + orientation[3] = -1; + orientation[7] = +1; + break; + case 13: + orientation[2] = -1; + orientation[4] = -1; + orientation[6] = -1; + break; + case 14: + orientation[2] = -1; + orientation[3] = +1; + orientation[7] = -1; + break; + case 15: + orientation[2] = -1; + orientation[4] = +1; + orientation[6] = +1; + break; + case 16: + orientation[0] = -1; + orientation[5] = +1; + orientation[7] = +1; + break; + case 17: + orientation[1] = -1; + orientation[5] = +1; + orientation[6] = -1; + break; + case 18: + orientation[0] = +1; + orientation[5] = -1; + orientation[7] = -1; + break; + case 19: + orientation[1] = -1; + orientation[5] = +1; + orientation[6] = +1; + break; + case 20: + orientation[0] = +1; + orientation[5] = -1; + orientation[7] = +1; + break; + case 21: + orientation[1] = -1; + orientation[5] = -1; + orientation[6] = +1; + break; + case 22: + orientation[0] = -1; + orientation[5] = -1; + orientation[7] = -1; + break; + case 23: + orientation[1] = +1; + orientation[5] = -1; + orientation[6] = -1; + break; + default: + MPL_LOGE("Invalid position %d\n", position); + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return INV_SUCCESS; +} + +static void PrintMountingOrientation( + const char * header, signed char *orientation) +{ + MPL_LOGV("%s:\n", header); + MPL_LOGV("\t[[%3d, %3d, %3d]\n", + orientation[0], orientation[1], orientation[2]); + MPL_LOGV("\t [%3d, %3d, %3d]\n", + orientation[3], orientation[4], orientation[5]); + MPL_LOGV("\t [%3d, %3d, %3d]]\n", + orientation[6], orientation[7], orientation[8]); +} + +/***************************** + * Accel Setup Functions * + *****************************/ + +static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 5; + break; + case PLATFORM_ID_ST_6AXIS: + position = 0; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *lis331_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS331; + return INV_SUCCESS; +} + +static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 1; + break; + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 3; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *lis3dh_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS3DH; + return result; +} + +static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 7; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *kxsd9_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXSD9; + return result; +} + +static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB_EVB: + position =0; + break; + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 7; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + position = 1; + break; +#endif + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 1; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *kxtf9_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXTF9; + return result; +} + +static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + position = 3; + break; + case PLATFORM_ID_MSB_V2: + position = 1; + break; + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *lsm303dlx_a_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LSM303; + return result; +} + +static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + position = 3; + break; +#endif + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *bma150_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA150; + return result; +} + +static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 0; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *bma222_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA222; + return result; +} + +static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 0; + break; + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 3; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *bma250_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA250; + return result; +} + +static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *adxl34x_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_ADXL34X; + return result; +} + + +static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 5; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *mma8450_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA8450; + return result; +} + + +static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 5; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_accel.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_accel = *mma845x_get_slave_descr(); +#endif + g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA845X; + return result; +} + + +/** + * @internal + * Sets up the orientation matrix according to how the gyro was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MANTIS_MSB: + position = 6; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_DRAGON_USB_DONGLE: + position = 1; + break; + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + case PLATFORM_ID_MANTIS_EVB: + position = 0; + break; + case PLATFORM_ID_MSB_V3: + position = 2; + break; + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + return INV_ERROR_INVALID_PARAMETER; + }; + + SetupOrientation(position, g_pdata_slave_accel.orientation); + /* Interrupt */ +#ifndef LINUX +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 + // g_slave_accel = // fixme *mpu6050_get_slave_descr(); +#endif +#endif + g_pdata_slave_accel.address = 0x68; + return result; +} + +/***************************** + Compass Setup Functions +******************************/ +static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MANTIS_MSB: + position = 2; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_USB_DONGLE: + position = 4; + break; +#endif + case PLATFORM_ID_SPIDER_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + position = 7; + break; + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V3: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 6; + break; + case PLATFORM_ID_DRAGON_USB_DONGLE: + case PLATFORM_ID_MSB_EVB: + position = 5; + break; + case PLATFORM_ID_MANTIS_EVB: + position = 4; + break; + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *ak8975_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AKM; + return result; +} + +static inv_error_t SetupCompassMMCCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 7; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *mmc314x_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_MMC314X; + return result; +} + +static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 4; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI304; +#ifndef LINUX + g_slave_compass = *ami30x_get_slave_descr(); +#endif + return result; +} + +static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 3; + break; + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *ami306_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI306; + return result; +} + +static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; +#ifdef WIN32 + case PLATFORM_ID_DONGLE: + position = 2; + break; +#endif + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *hmc5883_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; + return result; +} + + +static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_10AXIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#ifndef LINUX + g_slave_compass = *lsm303dlx_m_get_slave_descr(); + g_slave_compass.id = COMPASS_ID_LSM303DLH; +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; + return result; +} + +static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + position = 8; + break; + case PLATFORM_ID_MSB_V2: + position = 12; + break; + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *lsm303dlx_m_get_slave_descr(); + g_slave_compass.id = COMPASS_ID_LSM303DLM; +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; + return result; +} + +static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *yas530_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS530; + return result; +} + +static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 6; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *yas529_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS529; + return result; +} + + +static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 2; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *hscdtd002b_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX; + return result; +} + +static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId) +{ + inv_error_t result = INV_SUCCESS; + unsigned int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + position = 1; + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_MANTIS_MSB: + case PLATFORM_ID_MANTIS_USB_DONGLE: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + result = SetupOrientation(position, g_pdata_slave_compass.orientation); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#ifndef LINUX + g_slave_compass = *hscdtd004a_get_slave_descr(); +#endif + g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX; + return result; +} + + +/***************************** + Pressure Setup Functions +******************************/ +#if 0 +static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId) +{ + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation)); + + g_pdata_slave_pressure.bus = EXT_SLAVE_BUS_PRIMARY; +#ifndef LINUX + g_slave_pressure = *bma085_get_slave_descr(); +#endif + g_pdata_slave_pressure.address = PRESSURE_SLAVEADDR_BMA085; + return INV_SUCCESS; +} +#endif +/** + * @internal + * Sets up the orientation matrix according to how the part was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +static inv_error_t SetupAccelCalibration(unsigned short platformId, + unsigned short accelId) +{ + /*---- setup the accels ----*/ + switch(accelId) { + case ACCEL_ID_LSM303DLX: + SetupAccelLSM303Calibration(platformId); + break; + case ACCEL_ID_LIS331: + SetupAccelSTLIS331Calibration(platformId); + break; + case ACCEL_ID_KXSD9: + SetupAccelKionixKXSD9Calibration(platformId); + break; + case ACCEL_ID_KXTF9: + SetupAccelKionixKXTF9Calibration(platformId); + break; + case ACCEL_ID_BMA150: + SetupAccelBMA150Calibration(platformId); + break; + case ACCEL_ID_BMA222: + SetupAccelBMA222Calibration(platformId); + break; + case ACCEL_ID_BMA250: + SetupAccelBMA250Calibration(platformId); + break; + case ACCEL_ID_ADXL34X: + SetupAccelADXL34XCalibration(platformId); + break; + case ACCEL_ID_MMA8450: + SetupAccelMMA8450Calibration(platformId); + break; + case ACCEL_ID_MMA845X: + SetupAccelMMA845XCalibration(platformId); + break; + case ACCEL_ID_LIS3DH: + SetupAccelSTLIS3DHCalibration(platformId); + break; + case ACCEL_ID_MPU6050: + SetupAccelMPU6050Calibration(platformId); + break; + case ID_INVALID: + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) { + g_pdata_slave_accel.bus = EXT_SLAVE_BUS_SECONDARY; + } else if (accelId != ACCEL_ID_MPU6050) { + g_pdata_slave_accel.bus = EXT_SLAVE_BUS_PRIMARY; + } + +#ifndef WIN32 + if (accelId != ID_INVALID) + Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation); +#endif + + return INV_SUCCESS; +} + +/** + * @internal + * Sets up the orientation matrix according to how the part was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t SetupCompassCalibration(unsigned short platformId, + unsigned short compassId) +{ + /*---- setup the compass ----*/ + switch(compassId) { + case COMPASS_ID_AK8975: + SetupCompassAKM8975Calibration(platformId); + break; + case COMPASS_ID_AMI30X: + SetupCompassAMI304Calibration(platformId); + break; + case COMPASS_ID_AMI306: + SetupCompassAMI306Calibration(platformId); + break; + case COMPASS_ID_LSM303DLH: + SetupCompassLSM303DLHCalibration(platformId); + break; + case COMPASS_ID_LSM303DLM: + SetupCompassLSM303DLMCalibration(platformId); + break; + case COMPASS_ID_HMC5883: + SetupCompassHMC5883Calibration(platformId); + break; + case COMPASS_ID_YAS529: + SetupCompassYAS529Calibration(platformId); + break; + case COMPASS_ID_YAS530: + SetupCompassYAS530Calibration(platformId); + break; + case COMPASS_ID_MMC314X: + SetupCompassMMCCalibration(platformId); + break; + case COMPASS_ID_HSCDTD002B: + SetupCompassHSCDTD002BCalibration(platformId); + break; + case COMPASS_ID_HSCDTD004A: + SetupCompassHSCDTD004ACalibration(platformId); + break; + case ID_INVALID: + break; + default: + if (INV_ERROR_INVALID_PARAMETER) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + break; + } + + if (platformId == PLATFORM_ID_MSB_V2_MANTIS || + platformId == PLATFORM_ID_MANTIS_MSB || + platformId == PLATFORM_ID_MANTIS_USB_DONGLE || + platformId == PLATFORM_ID_MANTIS_PROTOTYPE || + platformId == PLATFORM_ID_DRAGON_PROTOTYPE) { + switch (compassId) { + case ID_INVALID: + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID; + break; + case COMPASS_ID_AK8975: + case COMPASS_ID_AMI306: + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_SECONDARY; + break; + default: + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY; + }; + } else { + g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY; + } + +#ifndef WIN32 + if (compassId != ID_INVALID) + Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation); +#endif + + return INV_SUCCESS; +} + +/** + * @internal + * Sets up the orientation matrix according to how the part was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +#if 0 +inv_error_t SetupPressureCalibration(unsigned short platformId, + unsigned short pressureId) +{ + inv_error_t result = INV_SUCCESS; + /*---- setup the compass ----*/ + switch(pressureId) { + case PRESSURE_ID_BMA085: + result = SetupPressureBMA085Calibration(platformId); + break; + default: + if (INV_ERROR_INVALID_PARAMETER) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + }; + + return result; +} +#endif +/** + * @internal + * Sets up the orientation matrix according to how the gyro was + * mounted. + * + * @param platforId Platform identification for mounting information + * @return INV_SUCCESS or non-zero error code + */ +static inv_error_t SetupGyroCalibration(unsigned short platformId) +{ + int position; + MPL_LOGV("Calibrating '%s'\n", __func__); + + /* Orientation */ + switch (platformId) { + case PLATFORM_ID_SPIDER_PROTOTYPE: + position = 2; + break; + case PLATFORM_ID_MSB: + case PLATFORM_ID_MSB_10AXIS: + case PLATFORM_ID_MANTIS_MSB: +#ifndef WIN32 + position = 4; +#else + position = 6; +#endif + break; + case PLATFORM_ID_DONGLE: + case PLATFORM_ID_MANTIS_USB_DONGLE: + position = 1; + break; + case PLATFORM_ID_DRAGON_USB_DONGLE: + position = 3; + break; + case PLATFORM_ID_MANTIS_PROTOTYPE: + case PLATFORM_ID_DRAGON_PROTOTYPE: + case PLATFORM_ID_ST_6AXIS: + case PLATFORM_ID_MSB_V2: + case PLATFORM_ID_MSB_V2_MANTIS: +#ifndef WIN32 + position = 2; +#else + position = 0; +#endif + break; + case PLATFORM_ID_MANTIS_EVB: + case PLATFORM_ID_MSB_EVB: + position = 0; + break; + case PLATFORM_ID_MSB_V3: + position = 2; + break; + default: + MPL_LOGE("Unsupported platform %d\n", platformId); + return INV_ERROR_INVALID_PARAMETER; + }; + + SetupOrientation(position, g_gyro_orientation); + + return INV_SUCCESS; +} + +/** + * @brief Setup the Hw orientation and full scale. + * @param platfromId + * an user defined Id to distinguish the Hw platform in + * use from others. + * @param accelId + * the accelerometer specific id, as specified in the MPL. + * @param compassId + * the compass specific id, as specified in the MPL. + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t SetupPlatform( + unsigned short platformId, + unsigned short accelId, + unsigned short compassId) +{ + int result; + + memset(&g_slave_accel, 0, sizeof(g_slave_accel)); + memset(&g_slave_compass, 0, sizeof(g_slave_compass)); +// memset(&g_slave_pressure, 0, sizeof(g_slave_pressure)); +// memset(&g_pdata, 0, sizeof(g_pdata)); + +#ifdef LINUX + /* On Linux initialize the global platform data with the driver defaults */ + { + void *mpu_handle; + int ii; + + struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES]; + slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel; + slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass; + //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure; + + pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel; + pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass; + //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure; + + MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n"); + result = inv_serial_open("/dev/mpu",&mpu_handle); + if (result) { + MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result); + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!slave[ii]) + continue; + slave[ii]->type = ii; + result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR, + slave[ii]); + if (result) + result = errno; + if(result == INV_ERROR_INVALID_MODULE) { + slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE); + return result; + } + } + //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata); + if (result) { + result = errno; + LOG_RESULT_LOCATION(result); + return result; + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + continue; + pdata_slave[ii]->type = ii; + result = ioctl( + (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA, + pdata_slave[ii]); + if (result) + result = errno; + if (result == INV_ERROR_INVALID_MODULE) { + pdata_slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + if (result) { + MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result); + } + inv_serial_close(mpu_handle); + } +#endif + + result = SetupGyroCalibration(platformId); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Gyroscope", g_gyro_orientation); + result = SetupAccelCalibration(platformId, accelId); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation); + result = SetupCompassCalibration(platformId, compassId); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation); +#if 0 + if (platformId == PLATFORM_ID_MSB_10AXIS) { + result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation); + } +#endif +#ifdef LINUX + /* On Linux override the orientation, level shifter etc */ + { + void *mpu_handle; + int ii; + struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES]; + slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel; + slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass; + //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure; + + pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; + pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel; + pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass; + //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure; + + MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n"); + result = inv_serial_open("/dev/mpu",&mpu_handle); + if (result) { + MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result); + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!slave[ii]) + continue; + slave[ii]->type = ii; + result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA, + slave[ii]); + if (result) + result = errno; + if (result == INV_ERROR_INVALID_MODULE) { + slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata); + if (result) { + result = errno; + LOG_RESULT_LOCATION(result); + return result; + } + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + continue; + pdata_slave[ii]->type = ii; + result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA, + pdata_slave[ii]); + if (result) + result = errno; + if (result == INV_ERROR_INVALID_MODULE) { + pdata_slave[ii] = NULL; + result = 0; + } else if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + if (result) { + MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result); + } + inv_serial_close(mpu_handle); + } +#endif + return INV_SUCCESS; +} + +/** + * @} + */ + + diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.h b/libsensors_iio/software/simple_apps/common/mlsetup.h new file mode 100644 index 0000000..06fa9f4 --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/mlsetup.h @@ -0,0 +1,52 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: mlsetup.h 6101 2011-09-29 00:30:33Z kkatingari $ + * + *******************************************************************************/ + +#ifndef MLSETUP_H +#define MLSETUP_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "linux/mpu.h" +#include "mltypes.h" + + enum mpu_platform_id { + PLATFORM_ID_INVALID = ID_INVALID, // 0 + PLATFORM_ID_MSB, // (0x0001) MSB (Multi sensors board) + PLATFORM_ID_ST_6AXIS, // (0x0002) 6 Axis with ST accelerometer + PLATFORM_ID_DONGLE, // (0x0003) 9 Axis USB dongle with + PLATFORM_ID_MANTIS_PROTOTYPE, // (0x0004) Mantis prototype board + PLATFORM_ID_MANTIS_MSB, // (0x0005) MSB with Mantis + PLATFORM_ID_MANTIS_USB_DONGLE,// (0x0006) Mantis and AKM on USB dongle. + PLATFORM_ID_MSB_10AXIS, // (0x0007) MSB with pressure sensor + PLATFORM_ID_DRAGON_PROTOTYPE, // (0x0008) Dragon prototype board + PLATFORM_ID_MSB_V2, // (0x0009) Version 2 MSB + PLATFORM_ID_MSB_V2_MANTIS, // (0x000A) Version 2 MSB with mantis + PLATFORM_ID_MANTIS_EVB, // (0x000B) Mantis EVB (shipped to cust.) + PLATFORM_ID_DRAGON_USB_DONGLE,// (0x000C) Dragon USB Dongle with Mantis Rev C + PLATFORM_ID_MSB_EVB, // (0X000D) MSB with 3050. + PLATFORM_ID_SPIDER_PROTOTYPE, + PLATFORM_ID_MSB_V3, + + NUM_PLATFORM_IDS + }; + // Main entry APIs +inv_error_t SetupPlatform(unsigned short platformId, + unsigned short accelSelection, + unsigned short compassSelection); + +#ifdef __cplusplus +} +#endif + +#endif /* MLSETUP_H */ diff --git a/libsensors_iio/software/simple_apps/common/slave.h b/libsensors_iio/software/simple_apps/common/slave.h new file mode 100644 index 0000000..7b40a8c --- /dev/null +++ b/libsensors_iio/software/simple_apps/common/slave.h @@ -0,0 +1,176 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: slave.h 5732 2011-07-07 01:11:34Z vbhatt $ + * + *******************************************************************************/ + +#ifndef SLAVE_H +#define SLAVE_H + +/** + * @addtogroup SLAVEDL + * + * @{ + * @file slave.h + * @brief Top level descriptions for Accelerometer support + * + */ + +#include "mltypes.h" +#include "linux/mpu.h" + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +/*--- default accel support - selection ---*/ +#define ACCEL_ST_LIS331 0 +#define ACCEL_KIONIX_KXTF9 1 +#define ACCEL_BOSCH 0 +#define ACCEL_ADI 0 + +#define ACCEL_SLAVEADDR_INVALID 0x00 + +#define ACCEL_SLAVEADDR_LIS331 0x18 +#define ACCEL_SLAVEADDR_LSM303 0x18 +#define ACCEL_SLAVEADDR_LIS3DH 0x18 +#define ACCEL_SLAVEADDR_KXSD9 0x18 +#define ACCEL_SLAVEADDR_KXTF9 0x0F +#define ACCEL_SLAVEADDR_BMA150 0x38 +#define ACCEL_SLAVEADDR_BMA222 0x08 +#define ACCEL_SLAVEADDR_BMA250 0x18 +#define ACCEL_SLAVEADDR_ADXL34X 0x53 +#define ACCEL_SLAVEADDR_ADXL34X_ALT 0x1D /* alternative addr */ +#define ACCEL_SLAVEADDR_MMA8450 0x1C +#define ACCEL_SLAVEADDR_MMA845X 0x1C + +#define ACCEL_SLAVEADDR_INVENSENSE 0x68 +/* + Define default accelerometer to use if no selection is made +*/ +#if ACCEL_ST_LIS331 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LIS331 +#define DEFAULT_ACCEL_ID ACCEL_ID_LIS331 +#endif + +#if ACCEL_ST_LSM303 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LSM303 +#define DEFAULT_ACCEL_ID ACCEL_ID_LSM303DLX +#endif + +#if ACCEL_KIONIX_KXSD9 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXSD9 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXSD9 +#endif + +#if ACCEL_KIONIX_KXTF9 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXTF9 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 +#endif + +#if ACCEL_BOSCH +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA150 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA150 +#endif + +#if ACCEL_BMA222 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA222 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA222 +#endif + +#if ACCEL_BOSCH +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA250 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA250 +#endif + +#if ACCEL_ADI +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_ADXL34X +#define DEFAULT_ACCEL_ID ACCEL_ID_ADXL34X +#endif + +#if ACCEL_MMA8450 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA8450 +#define DEFAULT_ACCEL_ID ACCEL_ID_MMA8450 +#endif + +#if ACCEL_MMA845X +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA845X +#define DEFAULT_ACCEL_ID ACCEL_ID_MMA845X +#endif + +/*--- if no default accelerometer was selected ---*/ +#ifndef DEFAULT_ACCEL_SLAVEADDR +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_INVALID +#endif + +#define USE_COMPASS_AICHI 0 +#define USE_COMPASS_AKM 0 +#define USE_COMPASS_YAS529 0 +#define USE_COMPASS_YAS530 0 +#define USE_COMPASS_HMC5883 0 +#define USE_COMPASS_MMC314X 0 +#define USE_COMPASS_HSCDTD002B 0 +#define USE_COMPASS_HSCDTD004A 0 + +#define COMPASS_SLAVEADDR_INVALID 0x00 +#define COMPASS_SLAVEADDR_AKM_BASE 0x0C +#define COMPASS_SLAVEADDR_AKM 0x0E +#define COMPASS_SLAVEADDR_AMI304 0x0E +#define COMPASS_SLAVEADDR_AMI305 0x0F /*Slave address for AMI 305/306*/ +#define COMPASS_SLAVEADDR_AMI306 0x0E /*Slave address for AMI 305/306*/ +#define COMPASS_SLAVEADDR_YAS529 0x2E +#define COMPASS_SLAVEADDR_YAS530 0x2E +#define COMPASS_SLAVEADDR_HMC5883 0x1E +#define COMPASS_SLAVEADDR_MMC314X 0x30 +#define COMPASS_SLAVEADDR_HSCDTD00XX 0x0C + +/* + Define default compass to use if no selection is made +*/ + #if USE_COMPASS_AKM + #define DEFAULT_COMPASS_TYPE COMPASS_ID_AK8975 + #endif + + #if USE_COMPASS_AICHI + #define DEFAULT_COMPASS_TYPE COMPASS_ID_AMI30X + #endif + + #if USE_COMPASS_YAS529 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS529 + #endif + + #if USE_COMPASS_YAS530 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS530 + #endif + + #if USE_COMPASS_HMC5883 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_HMC5883 + #endif + +#if USE_COMPASS_MMC314X +#define DEFAULT_COMPASS_TYPE COMPASS_ID_MMC314X +#endif + +#if USE_COMPASS_HSCDTD002B +#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD002B +#endif + +#if USE_COMPASS_HSCDTD004A +#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD004A +#endif + +#ifndef DEFAULT_COMPASS_TYPE +#define DEFAULT_COMPASS_TYPE ID_INVALID +#endif + + +#endif // SLAVE_H + +/** + * @} + */ diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared Binary files differnew file mode 100644 index 0000000..228abf7 --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk new file mode 100644 index 0000000..b1d881c --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk @@ -0,0 +1,109 @@ +EXEC = consoledmp$(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 += -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 + +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif + +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) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk new file mode 100644 index 0000000..b01fdfb --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk @@ -0,0 +1,23 @@ +#### filelist.mk for console_test #### + +# helper headers +HEADERS := $(COMMON_DIR)/external_hardware.h +HEADERS += $(COMMON_DIR)/fopenCMake.h +HEADERS += $(COMMON_DIR)/helper.h +HEADERS += $(COMMON_DIR)/mlerrorcode.h +HEADERS += $(COMMON_DIR)/mlsetup.h +HEADERS += $(COMMON_DIR)/slave.h + +HEADERS += $(HAL_DIR)/include/mlos.h +HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h + +# sources +SOURCES := $(APP_DIR)/console_test.c + +# helper sources +SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c +SOURCES += $(HAL_DIR)/linux/mlos_linux.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c new file mode 100644 index 0000000..e15b20d --- /dev/null +++ b/libsensors_iio/software/simple_apps/console/linux/console_test.c @@ -0,0 +1,742 @@ +/******************************************************************************* + * $Id: $ + ******************************************************************************/ + +/******************************************************************************* + * + * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved. + * + ******************************************************************************/ +//#include <linux/conio.h> +//#include <fcntl.h> +//#include <termios.h> +//#include <unistd.h> + +//#if 0 +#include <stdio.h> +#include <time.h> +#include <sys/select.h> +#include <unistd.h> +#include <string.h> +#include <sys/ioctl.h> +#include <fcntl.h> +#include <poll.h> +#include <stdlib.h> +#include <linux/input.h> + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "console_test" + +#include "mlos.h" +#include "invensense.h" +#include "invensense_adv.h" +#include "inv_sysfs_utils.h" +#include "ml_stored_data.h" +#include "ml_math_func.h" +#include "ml_load_dmp.h" +/*#else +#include <unistd.h> +#include <dirent.h> +#include <fcntl.h> +#include <stdio.h> +#include <errno.h> +#include <sys/stat.h> +#include <stdlib.h> +#include <features.h> +#include <dirent.h> +#include <string.h> +#include <poll.h> +#include <stddef.h> +#include <linux/input.h> +#include <time.h> +#include <linux/time.h> +#include "inv_sysfs_utils.h" +#include "invensense.h" +#include "invensense_adv.h" +#include "ml_stored_data.h" +#include "ml_math_func.h" +#include "ml_load_dmp.h" +#include "log.h" +#endif*/ + +/* TODO: add devices as needed. */ +#define ITG3500 (0) +#define MPU6050 (1) +#define BMA250 (2) +#define NUM_DEVICES (ITG3500 + MPU6050 + BMA250) + +#define DEVICE MPU6050 +#define DMP_IMAGE dmp_firmware_200_latest +#define SIX_AXES 6 +#define NINE_AXES 9 + +#if 0 +struct input_event { + struct timeval time; + __u16 type; + __u16 code; + __s32 value; +}; +#endif + +/* TODO: Add paths to other attributes. + * TODO: Input device paths depend on the module installation order. + */ +const struct inv_sysfs_names_s filenames[NUM_DEVICES] = { + { /* ITG3500 */ + .buffer = "/dev/input/event0", + .enable = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable", + .raw_data = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro", + .temperature = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature", + .fifo_rate = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate", + .power_state = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state", + .fsr = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR", + .lpf = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf", + .scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale", + .temp_scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale", + .temp_offset = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset", + .self_test = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test", + .accel_en = NULL, + .accel_fifo_en = NULL, + .accel_fs = NULL, + .clock_source = NULL, + .early_suspend_en = NULL, + .firmware_loaded = NULL, + .gyro_en = NULL, + .gyro_fifo_en = NULL, + .key = NULL, + .raw_accel = NULL, + .reg_dump = NULL, + .tap_on = NULL, + .dmp_firmware = NULL + }, + + { /* MPU6050 */ + .buffer = "/dev/input/event0", + .enable = "/sys/class/invensense/mpu/enable", + .raw_data = "/sys/class/invensense/mpu/raw_gyro", + .temperature = "/sys/class/invensense/mpu/temperature", + .fifo_rate = "/sys/class/invensense/mpu/fifo_rate", + .power_state = "/sys/class/invensense/mpu/power_state", + .fsr = "/sys/class/invensense/mpu/FSR", + .lpf = "/sys/class/invensense/mpu/lpf", + .scale = "/sys/class/invensense/mpu/gyro_scale", + .temp_scale = "/sys/class/invensense/mpu/temp_scale", + .temp_offset = "/sys/class/invensense/mpu/temp_offset", + .self_test = "/sys/class/invensense/mpu/self_test", + .accel_en = "/sys/class/invensense/mpu/accl_enable", + .accel_fifo_en = "/sys/class/invensense/mpu/accl_fifo_enable", + .accel_fs = "/sys/class/invensense/mpu/accl_fs", + .clock_source = "/sys/class/invensense/mpu/clock_source", + .early_suspend_en = "/sys/class/invensense/mpu/early_suspend_enable", + .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded", + .gyro_en = "/sys/class/invensense/mpu/gyro_enable", + .gyro_fifo_en = "/sys/class/invensense/mpu/gyro_fifo_enable", + .key = "/sys/class/invensense/mpu/key", + .raw_accel = "/sys/class/invensense/mpu/raw_accl", + .reg_dump = "/sys/class/invensense/mpu/reg_dump", + .tap_on = "/sys/class/invensense/mpu/tap_on", + .dmp_firmware = "/sys/class/invensense/mpu/dmp_firmware" + }, + + { /* BMA250 */ + .buffer = "/dev/input/input/event1", + .enable = "/sys/devices/virtual/input/input1/enable", + .raw_data = "/sys/devices/virtual/input/input1/value", + .temperature = NULL, + .fifo_rate = NULL, + .power_state = NULL, + .fsr = NULL, + .lpf = NULL, + .scale = NULL, + .temp_scale = NULL, + .temp_offset = NULL, + .self_test = NULL, + .accel_en = NULL, + .accel_fifo_en = NULL, + .accel_fs = NULL, + .clock_source = NULL, + .early_suspend_en = NULL, + .firmware_loaded = NULL, + .gyro_en = NULL, + .gyro_fifo_en = NULL, + .key = NULL, + .raw_accel = NULL, + .reg_dump = NULL, + .tap_on = NULL, + .dmp_firmware = NULL + } +}; + +static void (*s_func_cb) (void); + +int inv_read_data(char *names, char *data) +{ + char str[8]; + int count; + short s_data; + + count = inv_sysfs_read((char*)names, sizeof(str), str); + if (count < 0) + return count; + count = sscanf(str, "%hd", &s_data); + *data = s_data; + if (count < 1) + return -EAGAIN; + return count; + +} + +void fifoCB(void) +{ + if (1) { + float gyro[3]; + float accel[3]; + float orient[3]; + float rv[3]; + + int8_t accuracy; + inv_time_t timestamp; + + printf("/*************************************************\n"); + inv_get_sensor_type_gyroscope(gyro, &accuracy, ×tamp); + printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n", + gyro[0], + gyro[1], + gyro[2], + accuracy, + timestamp); + + inv_get_sensor_type_accelerometer(accel, &accuracy, ×tamp); + printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n", + accel[0], + accel[1], + accel[2], + accuracy, + timestamp); + + inv_get_sensor_type_rotation_vector(rv, &accuracy, ×tamp); + printf("RV %7.3f %7.3f %7.3f %5d %9lld\n", + rv[0],rv[1],rv[2],accuracy,timestamp); + + inv_get_sensor_type_orientation(orient, &accuracy, ×tamp); + printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n", + orient[0],orient[1],orient[2],accuracy,timestamp); + printf("/*************************************************\n"); + } +} + +unsigned short orient; +signed char g_gyro_orientation[9] = {1, 0, 0, + 0, 1, 0, + 0, 0, 1}; + +signed char g_accel_orientation[9] = {-1, 0, 0, + 0, -1, 0, + 0, 0, 1}; +float scale; +float range; +long sens; + + + +short mTempOffset = 0; +short mTempScale = 0; +bool mFirstRead = 1; + +/******************* FUNCTIONS *******************************/ +#if 0 +static unsigned short inv_row_2_scale(const signed char *row) +{ + unsigned short b; + + if (row[0] > 0) + b = 0; + else if (row[0] < 0) + b = 4; + else if (row[1] > 0) + b = 1; + else if (row[1] < 0) + b = 5; + else if (row[2] > 0) + b = 2; + else if (row[2] < 0) + b = 6; + else + b = 7; // error + return b; +} +#endif + +inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)) +{ + s_func_cb = func_cb; + return INV_SUCCESS; +} + +/** + * @brief Keyboard hit function. + */ +int kbhit(void) +{ +#if 1 + struct timeval tv; + fd_set read_fd; + + tv.tv_sec=0; + tv.tv_usec=0; + FD_ZERO(&read_fd); + FD_SET(0,&read_fd); + + if (select(1, &read_fd, NULL, NULL, &tv) == -1) + return 0; + + if (FD_ISSET(0,&read_fd)) + return 1; + + return 0; +#else + struct timeval tv; + fd_set rdfs; + + tv.tv_sec = 0; + tv.tv_usec = 0; + + FD_ZERO(&rdfs); + FD_SET (STDIN_FILENO, &rdfs); + + select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv); + return FD_ISSET(STDIN_FILENO, &rdfs); +#endif +} + +inv_error_t inv_constructor_default_enable() +{ + inv_error_t result; + + result = inv_enable_quaternion(); + if (result) { + if (result == INV_ERROR_NOT_AUTHORIZED) { + LOGE("Enable Quaternion failed: not authorized"); + } + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_enable_motion_no_motion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_enable_gyro_tc(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_enable_hal_outputs(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_enable_9x_sensor_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +int read_attribute_sensor(int fd, char *data, unsigned int size) +{ + int count = 0; + if (fd >=0) { + count = read(fd, data, size); + if(count < 0) { + MPL_LOGE("read fails with error code=%d", count); + } + close(fd); + } + return count; +} + +int inv_read_temperature(long long *data) +{ + int count = 0; + int fd; + + if(mFirstRead) { + char buf[4]; + fd = open(filenames[ITG3500].temp_scale, O_RDONLY); + if(fd < 0) { + MPL_LOGE("errors opening tempscale"); + return -1; + } + + memset(buf, 0, sizeof(buf)); + + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 0) { + MPL_LOGE("errors reading temp_scale"); + return -1; + } + + count = sscanf(buf, "%hd", &mTempScale); + if(count < 1) + return -1; + MPL_LOGI("temp scale = %d", mTempScale); + + fd = open(filenames[ITG3500].temp_offset, O_RDONLY); + if(fd < 0) { + MPL_LOGE("errors opening tempoffset"); + return -1; + } + + memset(buf, 0, sizeof(buf)); + + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 0) { + MPL_LOGE("errors reading temp_offset"); + return -1; + } + + count = sscanf(buf, "%hd", &mTempOffset); + if(count < 1) + return -1; + MPL_LOGI("temp offset = %d", mTempOffset); + + mFirstRead = false; + } + + char raw_buf[25]; + short raw; + long long timestamp; + fd = open(filenames[ITG3500].temperature, O_RDONLY); + if(fd < 0) { + MPL_LOGE("errors opening temperature"); + return -1; + } + + memset(raw_buf, 0, sizeof(raw_buf)); + + count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); + if(count < 0) { + MPL_LOGE("errors reading temperature"); + return -1; + } + count = sscanf(raw_buf, "%hd%lld", &raw, ×tamp); + if(count < -1) + return -1; + MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp); + MPL_LOGI("temperature offset = %d", mTempOffset); + MPL_LOGI("temperature scale = %d", mTempScale); + int adjuster = 35 + ((raw-mTempOffset)/mTempScale); + MPL_LOGI("pre-scaled temperature = %d", adjuster); + MPL_LOGI("adjusted temperature = %d", adjuster*65536); + //data[0] = adjuster * 65536; + data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f; + data[1] = timestamp; + return 0; +} + +int self_test(void) +{ + int err = 0; + char str[50]; + char x[9], y[9], z[9]; + char pass[2]; + int fd; + + fd = open((char*)filenames[DEVICE].self_test, O_RDONLY); + if(fd < 0) { + return fd; + } + memset(str, 0, sizeof(str)); + err = read_attribute_sensor(fd, str, sizeof(str)); + if(err < 0) { + return err; + } + MPL_LOGI("self_test result: %s", str); + printf("Self test result: %s ", str); + err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass); + if(err < 1) { + return err; + } + MPL_LOGI("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); + //printf("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); + if (atoi(pass)) { + MPL_LOGI("Result : PASS (1)"); + printf("----> PASS (1)\n"); + } else { + MPL_LOGI("Result : FAIL (0)"); + printf("----> FAIL (0)\n"); + } + return err; +} + +/******************************************************************************* + ******************************* MAIN ****************************************** + ******************************************************************************/ + +/** + * @brief Main function + */ +int main(int argc, char *argv[]) +{ + int key = 0; + int ready; + long accel[3]; + short gyro[3]; + long long timestamp = 0; + inv_error_t result; + + char data; + unsigned char i; + int fd, bytes_read; + struct pollfd pfd; + unsigned long long time_stamp; + unsigned int time_H; + struct input_event ev[100]; +#ifdef INV_PLAYBACK_DBG + int logging = false; + FILE *logfile = NULL; +#endif + + result = inv_init_mpl(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + // Master Enabling. This also turns on power_state + if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0) + printf("ERR- Failed to enable event generation\n"); + else { + inv_read_data((char *)filenames[DEVICE].enable, &data); + printf("Event enable= %d\n", data); + } + + // Power ON - No need after master enable above but do it anyway + if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0) + printf("ERR- Failed to set power state=1\n"); + else { + inv_read_data((char *)filenames[DEVICE].power_state, &data); + printf("Power state: %d\n", data); + } + + // Turn on tap + if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) { + printf("ERR- Failed to enable Tap On\n"); + } + else { + inv_read_data((char *)filenames[DEVICE].tap_on, &data); + printf("Tap-on: %d\n", data); + } + + // Program DMP code. No longer required to enable tap-on first + if ((result = + inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) { + printf("ERR- Failed to initiate DMP re-programming %d\n",result); + } else { + if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) { + printf("ERR- Failed file open to write DMP\n"); + close(fd); + exit(0); + } else { + // Program 200Hz version + //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE)); + //printf("Downloaded %d byte(s) to DMP\n", result); + result = inv_load_dmp(fd); + //LOG_RESULT_LOCATION(result); + close(fd); + } + } + + // Query DMP running. For now check by 'firmware_loaded' status + if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) { + printf("ERR- Failed to read 'firmware_loaded'\n"); + } else { + printf("Firmware Loaded/ DMP running: %d\n", data); + } + + inv_set_fifo_processed_callback(fifoCB); + result = inv_constructor_default_enable(); + result = inv_start_mpl(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else { + printf ("MPL started\n"); + } + + /* Gyro Setup */ + orient = inv_orientation_matrix_to_scalar(g_gyro_orientation); + inv_set_gyro_orientation_and_scale(orient,2000L<<15); + + /* Accel Setup */ + orient = inv_orientation_matrix_to_scalar(g_accel_orientation); + /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data. + * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits. + * 2 * 1L<<15 * 1<<6 == 1LL<<22 + */ + inv_set_accel_orientation_and_scale(orient, 1LL<<22); + + // Enable Gyro + if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0) + printf("ERR- Failed to enable Gyro\n"); + else { + inv_read_data((char *)filenames[DEVICE].gyro_en, &data); + printf("Gyro enable: %d\n", data); + } + + // Enable Accel + if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0) + printf("ERR- Failed to enable Accel\n"); + else { + inv_read_data((char *)filenames[DEVICE].accel_en, &data); + printf("Accel enable: %d\n", data); + } + + // polling for data + fd = open(filenames[DEVICE].buffer, O_RDONLY); + if(fd < 0) { + MPL_LOGE("Cannot open device event buffer"); + } + + pfd.fd = fd; + pfd.events = POLLIN; + + while (1) { + + result = kbhit(); + if (result) { + key = getchar(); + } else { + key = 0; + } + if (key == 'l') { + MPL_LOGI(" 'l' - load calibration file"); + inv_load_calibration(); + } + if (key == 't') { + MPL_LOGI(" 't' - self test"); + self_test(); + } + if (key == 'q') { + MPL_LOGI(" 'q' - store calibration file"); + inv_store_calibration(); + break; + } +#ifdef INV_PLAYBACK_DBG + if (key == 's') { + if (!logging) { + MPL_LOGI(" 's' - toggle logging on"); + logfile = fopen("/data/playback.bin", "wb"); + if (logfile) { + inv_turn_on_data_logging(logfile); + logging = true; + } else { + MPL_LOGI("Error : " + "cannot open log file '/data/playback.bin'"); + } + } else { + MPL_LOGI(" 's' - toggle logging off"); + inv_turn_off_data_logging(); + fclose(logfile); + logging = false; + } + break; + } +#endif + + ready = poll(&pfd, 1, 100); + if (ready) { + bytes_read = read_attribute_sensor(fd, (char *)ev, + sizeof(struct input_event) * SIX_AXES); + //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES); + if (bytes_read > 0) { + int executed; + + for (i = 0; i < bytes_read / sizeof(struct input_event); i++) { + if (ev[i].type == EV_REL) { + switch (ev[i].code) { + case REL_X: + printf("REL_X\n"); + gyro[0]= ev[i].value; //Gyro X + printf("Gyro X:%5d ", gyro[0]); + break; + case REL_Y: + printf("REL_Y\n"); + gyro[1]= ev[i].value; //Gyro Y + printf("Gyro Y:%5d ", gyro[1]); + break; + case REL_Z: + printf("REL_Z\n"); + gyro[2]= ev[i].value; //Gyro Z + printf("Gyro Z:%5d ", gyro[2]); + break; + case REL_RX: + printf("REL_RX\n"); + accel[0]= ev[i].value; //Accel X + printf("Accl X:%5ld ", accel[0]); + break; + case REL_RY: + printf("REL_RY\n"); + accel[1]= ev[i].value; //Accel Y + printf("Accl Y:%5ld ", accel[1]); + break; + case REL_RZ: + printf("REL_RZ\n"); + accel[2]= ev[i].value; //Accel Z + printf("Accl Z:%5ld ", accel[2]); + break; + case REL_MISC: + time_H= ev[i].value; + break; + case REL_WHEEL: + time_stamp = ((unsigned long long)(time_H) << 32) + + (unsigned int)ev[i].value; + break; + default: + printf("ERR- Un-recognized event code: %5d ", ev[i].code); + break; + } + } else { +#if 0 + clock_gettime(CLOCK_MONOTONIC, &timer); + curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL; + printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL); +#endif + } + } + + // build & process gyro + accel data + result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed); + if (result) { + LOG_RESULT_LOCATION(result); + } else if ((result = inv_build_accel(accel, 0, + (inv_time_t)timestamp, + &executed))) { + LOG_RESULT_LOCATION(result); + } + if (executed) { + printf("Exec on data Ok\n"); + s_func_cb(); + } + + } else { + //printf ("ERR- No data!\n"); + } + + } else { MPL_LOGV("Device not ready"); } + } + close(fd); + +#ifdef INV_PLAYBACK_DBG + if (logging) { + inv_turn_off_data_logging(); + fclose(logfile); + } +#endif + + return 0; +} diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared Binary files differnew file mode 100644 index 0000000..5d52b21 --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk new file mode 100644 index 0000000..7f6cc43 --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk @@ -0,0 +1,109 @@ +EXEC = input_gyro$(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 += -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 + +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif + +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) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk new file mode 100644 index 0000000..0936212 --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk @@ -0,0 +1,13 @@ +#### filelist.mk for input_gyro #### + +# helper headers +HEADERS := $(MPL_DIR)/authenticate.h +#HEADERS += + +# sources +SOURCES := $(APP_DIR)/test_input_gyro.c + +INV_SOURCES += $(SOURCES) + +#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux +VPATH += $(APP_DIR) diff --git a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c new file mode 100644 index 0000000..6fa9aab --- /dev/null +++ b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c @@ -0,0 +1,485 @@ +/* + * input interface testing + */ +#include <sys/time.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <time.h> +#include <fcntl.h> +#include <sys/ioctl.h> +#include <linux/input.h> +#include <linux/config.h> +#include <stdio.h> +#include <stdlib.h> +#include "linux/ml_sysfs_helper.h" +#include "authenticate.h" +#include "ml_load_dmp.h" + +#if 0 +struct input_event { + struct timeval time; + __u16 type; + __u16 code; + __s32 value; +}; +#endif + +void HandleOrient(int orient) +{ + if (orient & 0x01) + printf("INV_X_UP\n"); + if (orient & 0x02) + printf("INV_X_DOWN\n"); + if (orient & 0x04) + printf("INV_Y_UP\n"); + if (orient & 0x08) + printf("INV_Y_DOWN\n"); + if (orient & 0x10) + printf("INV_Z_UP\n"); + if (orient & 0x20) + printf("INV_Z_DOWN\n"); + if (orient & 0x40) + printf("INV_ORIENTATION_FLIP\n"); +} + +void HandleTap(int tap) +{ + int tap_dir = tap/8; + int tap_num = tap%8 + 1; + + switch (tap_dir) { + case 1: + printf("INV_TAP_AXIS_X_POS\n"); + break; + case 2: + printf("INV_TAP_AXIS_X_NEG\n"); + break; + case 3: + printf("INV_TAP_AXIS_Y_POS\n"); + break; + case 4: + printf("INV_TAP_AXIS_Y_NEG\n"); + break; + case 5: + printf("INV_TAP_AXIS_Z_POS\n"); + break; + case 6: + printf("INV_TAP_AXIS_Z_NEG\n"); + break; + default: + break; + } + printf("Tap number: %d\n", tap_num); +} + +static void read_compass(int event_number) +{ + int ev_size, ret_byte, ii; + int fd = -1; + struct input_event ev[10]; + char name[64]; + char file_name[64]; + unsigned int RX; + unsigned long long time0, time1, time2; + struct timespec tsp; + ev_size = sizeof(struct input_event); + sprintf(file_name, "/dev/input/event%d", event_number); + if ((fd = open(file_name, O_RDONLY)) < 0 ) { + printf("fail to open compass\n"); + return; + } + + /* NOTE: Use this to pass device name to HAL. */ + ioctl (fd, EVIOCGNAME (sizeof (name)), name); + printf ("Reading From : (%s)\n", name); + while (1) { + clock_gettime(CLOCK_MONOTONIC, &tsp); + /*read compass data here */ + if(fd > 0){ + ret_byte = read(fd, ev, ev_size); + } else { + ret_byte = -1; + } + time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL; + if (ret_byte < 0) + continue; + for (ii = 0; ii < ret_byte/ev_size; ii++) { + if(EV_REL != ev[ii].type) { + time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL; + printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0)); + continue; + } + switch (ev[ii].code) { + case REL_X: + printf("CX:%5d ", ev[ii].value); + break; + case REL_Y: + printf("CY:%5d ", ev[ii].value); + break; + case REL_Z: + printf("CZ:%5d ", ev[ii].value); + break; + case REL_MISC: + RX = ev[ii].value; + break; + case REL_WHEEL: + time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value; + time1 = time1/1000000; + printf("time1: %lld ", time1); + break; + default: + printf("GES?: %5d ", ev[ii].code); + break; + } + } + } + close(fd); +} + +static void read_gesture(int num) +{ + int ev_size, ret_byte, ii; + int fd = -1; + struct input_event ev[10]; + char name[64]; + char file_name[64]; + unsigned long long time; + struct timespec tsp; + ev_size = sizeof(struct input_event); + sprintf(file_name, "/dev/input/event%d", num); + MPL_LOGI("%s\n", file_name); + if ((fd = open(file_name, O_RDONLY)) < 0 ) { + printf("fail to open gusture.\n"); + return; + } + + /* NOTE: Use this to pass device name to HAL. */ + ioctl (fd, EVIOCGNAME (sizeof (name)), name); + printf ("Reading From : (%s)\n", name); + while(1){ + clock_gettime(CLOCK_MONOTONIC, &tsp); + if(fd > 0){ + ret_byte = read(fd, ev, ev_size); + } else { + ret_byte = -1; + } + time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL; + //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3); + if (ret_byte < 0) + continue; + for (ii = 0; ii < ret_byte/ev_size; ii++) { + if(EV_REL != ev[ii].type) { + time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL; + printf("mono=%lld\n", time); + continue; + } + switch (ev[ii].code) { + case REL_RX: + printf("GESX:%5x\n", ev[ii].value); + HandleTap(ev[ii].value); + break; + case REL_RY: + printf("GESY:%5x\n", ev[ii].value); + HandleOrient(ev[ii].value); + break; + case REL_RZ: + printf("FLICK:%5x\n", ev[ii].value); + break; + default: + printf("?: %5d ", ev[ii].code); + break; + } + } + } +} + +static void read_gyro_accel(int num) +{ + int ev_size, ret_byte, ii; + int fd = -1; + unsigned int RX; + struct input_event ev[10]; + char name[64]; + char file_name[64]; + unsigned long long time0, time1, time2; + struct timespec tsp; + ev_size = sizeof(struct input_event); + sprintf(file_name, "/dev/input/event%d", num); + if ((fd = open(file_name, O_RDONLY)) < 0 ) { + printf("fail to open gyro/accel\n"); + return; + } + + /* NOTE: Use this to pass device name to HAL. */ + ioctl (fd, EVIOCGNAME (sizeof (name)), name); + printf ("Reading From : (%s)\n", name); + while (1){ + //usleep(20000); + ret_byte = read(fd, ev, ev_size); + if (ret_byte < 0) + continue; + //ret_byte = 0; + + for (ii = 0; ii < ret_byte/ev_size; ii++) { + if(EV_REL != ev[ii].type) { + time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL; + printf("T: %lld diff=%d ", time0, (int)(time1 - time0)); + clock_gettime(CLOCK_MONOTONIC, &tsp); + time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL; + printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2)); + continue; + } + switch (ev[ii].code) { + case REL_X: + printf("GX:%5d ", ev[ii].value); + break; + case REL_Y: + printf("GY:%5d ", ev[ii].value); + break; + case REL_Z: + printf("GZ:%5d ", ev[ii].value); + break; + case REL_RX: + printf("AX:%5d ", ev[ii].value); + break; + case REL_RY: + printf("AY:%5d ", ev[ii].value); + break; + case REL_RZ: + printf("AZ:%5d ", ev[ii].value); + break; + case REL_MISC: + RX = ev[ii].value; + break; + case REL_WHEEL: + time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value; + time1 = time1/1000000; + printf("time1: %lld ", time1); + break; + default: + printf("?: %5d ", ev[ii].code); + break; + } + } + } + close(fd); +} +int inv_sysfs_write(char *filename, long data) +{ + FILE *fp; + int count; + + if (!filename) + return -1; + fp = fopen(filename, "w"); + if (!fp) + return -errno; + count = fprintf(fp, "%ld", data); + fclose(fp); + return count; +} +int inv_sysfs_read(char *filename, long num_bytes, char *data) +{ + FILE *fp; + int count; + + if (!filename) + return -1; + fp = fopen(filename, "r"); + if (!fp) + return -errno; + count = fread(data, 1, num_bytes, fp); + fclose(fp); + return count; +} + +void enable_flick(char *p) +{ + char sysfs_file[200]; + printf("flick:%s\n", p); + sprintf(sysfs_file, "%s/flick_int_on", p); + inv_sysfs_write(sysfs_file, 1); + sprintf(sysfs_file, "%s/flick_upper", p); + inv_sysfs_write(sysfs_file, 3147790); + sprintf(sysfs_file, "%s/flick_lower", p); + inv_sysfs_write(sysfs_file, -3147790); + sprintf(sysfs_file, "%s/flick_counter", p); + inv_sysfs_write(sysfs_file, 50); + sprintf(sysfs_file, "%s/flick_message_on", p); + inv_sysfs_write(sysfs_file, 0); + sprintf(sysfs_file, "%s/flick_axis", p); + inv_sysfs_write(sysfs_file, 2); +} + +void setup_dmp(char *sysfs_path) +{ + char sysfs_file[200]; + char firmware_loaded[200], dmp_path[200]; + char dd[10]; + + inv_get_dmpfile(dmp_path); + sprintf(sysfs_file, "%s/fifo_rate", sysfs_path); + inv_sysfs_write(sysfs_file, 200); + sprintf(sysfs_file, "%s/FSR", sysfs_path); + inv_sysfs_write(sysfs_file, 2000); + sprintf(sysfs_file, "%s/accl_fs", sysfs_path); + inv_sysfs_write(sysfs_file, 4); + /* + sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded"); + printf("%s\n", firmware_loaded); + inv_sysfs_write(firmware_loaded, 0); + inv_sysfs_read(firmware_loaded, 1, dd); + printf("beforefirmware_loaded=%c\n", dd[0]); + + if ((fd = open(dmp_path, O_WRONLY)) < 0 ) { + perror("dmp fail"); + } + inv_load_dmp(fd); + close(fd); + */ + inv_sysfs_read(firmware_loaded, 1, dd); + printf("firmware_loaded=%c\n", dd[0]); +} +void read_pedometer(char *sysfs_path){ + int steps; + char sysfs_file[200]; + char dd[4]; + sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path); + inv_sysfs_read(sysfs_file, 4, dd); + steps = dd[0] << 8 | dd[1]; + printf("fff=%d\n", steps); +} +/* The running sequence: + "input_gyro 2 &". + This will setup the dmp firmware and let it run on background. + tap and flick will work at this time. + To see accelerometer data and gyro data. + type : + "input_gyro ". + This will print out gyro data and accelerometer data + To see Compass data + type: + "input_gyro 1" */ + +int main(int argc, char *argv[]) +{ + unsigned int RX, i, sel; + unsigned char key[16]; + struct timeval tv; + struct timespec tsp0, tsp1, tsp2, tsp3; + int event_num; + char sysfs_path[200]; + char chip_name[20]; + char sysfs_file[200]; + if (INV_SUCCESS != inv_check_key()) { + printf("key check fail\n"); + exit(0); + }else + printf("key authenticated\n"); + + for(i=0;i<16;i++){ + key[i] = 0xff; + } + RX = inv_get_sysfs_key(key); + if(RX == INV_SUCCESS){ + for(i=0;i<16;i++){ + printf("%d, ", key[i]); + } + printf("\n"); + }else{ + printf("get key failed\n"); + } + memset(sysfs_path, 0, 200); + memset(sysfs_file, 0, 200); + memset(chip_name, 0, 20); + inv_get_sysfs_path(sysfs_path); + inv_get_chip_name(chip_name); + printf("sysfs path: %s\n", sysfs_path); + printf("chip name: %s\n", chip_name); + /*set up driver*/ + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 0); + sprintf(sysfs_file, "%s/power_state", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if ((getuid ()) != 0) + printf ("You are not root! This may not work...\n"); + + if(argc ==2 ) + sel = argv[1][0] - 0x30; + else + sel = 0; + switch(sel){ + case 0: + printf("-------------------------------\n"); + printf("--- log gyro and accel data ---\n"); + printf("-------------------------------\n"); + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if(inv_get_handler_number(chip_name, &event_num) < 0) + printf("mpu not installed\n"); + else + read_gyro_accel(event_num); + break; + + case 1: + printf("------------------------\n"); + printf("--- log compass data ---\n"); + printf("------------------------\n"); + sprintf(sysfs_file, "%s/compass_enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if(inv_get_handler_number("INV_COMPASS", &event_num) < 0) + printf("compass is not enabled\n"); + else + read_compass(event_num); + break; + + case 2: + printf("--------------------\n"); + printf("--- log gestures ---\n"); + printf("--------------------\n"); + setup_dmp(sysfs_path); + enable_flick(sysfs_path); + sprintf(sysfs_file, "%s/tap_on", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + sprintf(sysfs_file, "%s/enable", sysfs_path); + inv_sysfs_write(sysfs_file, 1); + if(inv_get_handler_number("INV_DMP", &event_num) < 0) + printf("DMP not enabled\n"); + else + read_gesture(event_num); + break; + + case 3: + printf("-----------------\n"); + printf("--- pedometer ---\n"); + printf("-----------------\n"); + read_pedometer(sysfs_path); + break; + + default: + printf("error choice\n"); + break; + } + + gettimeofday(&tv, NULL); + clock_gettime(CLOCK_MONOTONIC, &tsp1); + clock_gettime(CLOCK_REALTIME, &tsp0); + + clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2); + clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3); + //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC, CLOCK_REALTIME, + // CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID); + //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec); + //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec); + //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec); + //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec); + + //ioctl (fd, EVIOCGNAME (sizeof (name)), name); + //printf ("Reading From : %s (%s)\n", argv[1], name); + + + return 0; +} + diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared Binary files differnew file mode 100644 index 0000000..33c9eef --- /dev/null +++ b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk new file mode 100644 index 0000000..3a055cc --- /dev/null +++ b/libsensors_iio/software/simple_apps/self_test/build/android/shared.mk @@ -0,0 +1,109 @@ +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 += -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 + +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif + +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) $(PRE_LFLAGS) $(INV_OBJS_DST) -o $(EXEC) $(LFLAGS) $(LLINK) $(INV_LIBS) $(LLINK) $(LRPATH) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/libsensors_iio/software/simple_apps/self_test/build/filelist.mk b/libsensors_iio/software/simple_apps/self_test/build/filelist.mk new file mode 100644 index 0000000..492f47e --- /dev/null +++ b/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/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c new file mode 100644 index 0000000..4f9996c --- /dev/null +++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c @@ -0,0 +1,264 @@ +/** + * Self Test application for Invensense's MPU6050/MPU9150. + */ + +#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" + +#ifndef ABS +#define ABS(x)(((x) >= 0) ? (x) : -(x)) +#endif + +/** Change this key if the data being stored by this file changes */ +#define INV_DB_SAVE_KEY 53394 + +#define FALSE 0 +#define TRUE 1 + +#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; + +struct inv_sysfs_names_s { + char *enable; + char *power_state; + char *self_test; + char *temperature; + char *accl_bias; +}; + +const struct inv_sysfs_names_s mpu= { + /* MPU6050 & MPU9150 */ + .enable = "/sys/class/invensense/mpu/enable", + .power_state = "/sys/class/invensense/mpu/power_state", + .self_test = "/sys/class/invensense/mpu/self_test", + .temperature = "/sys/class/invensense/mpu/temperature", + .accl_bias = "/sys/class/invensense/mpu/accl_bias" +}; + +struct inv_db_save_t { + /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ + long compass_bias[3]; + /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ + long gyro_bias[3]; + /** Temperature when *gyro_bias was stored. */ + long gyro_temp; + /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ + long accel_bias[3]; + /** Temperature when accel bias was stored. */ + long accel_temp; + long gyro_temp_slope[3]; +}; + +static struct inv_db_save_t save_data; + +/** 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; +} + +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; +} + +/** + * Main Self test + */ +int main(int argc, char **argv) +{ + FILE *fptr; + int self_test_status; + inv_error_t result; + bias_dtype gyro_bias[3]; + bias_dtype accel_bias[3]; + int axis = 0; + size_t packet_sz; + int axis_sign = 1; + unsigned char *buffer; + long timestamp; + int temperature=0; + + // Initialize storage manager + inv_init_storage_manager(); + + // Clear out data. + memset(&save_data, 0, sizeof(save_data)); + memset(gyro_bias,0, sizeof(gyro_bias)); + memset(accel_bias,0, sizeof(accel_bias)); + + // 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); + + // Power ON MPUxxxx chip + if (inv_sysfs_write(mpu.power_state, 1) <0) { + printf("ERR- Failed to set power state=1\n"); + } else { + // Note: Driver turns on power automatically when self-test invoked + } + + fptr = fopen(mpu.self_test, "r"); + if (fptr != NULL) { + // Invoke self-test and read gyro bias + fscanf(fptr, "%d,%d,%d,%d", + &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status); + + printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", + (self_test_status & GYRO_PASS_STATUS_BIT), + (self_test_status & ACCEL_PASS_STATUS_BIT) >>1, + (self_test_status & COMPASS_PASS_STATUS_BIT) >>2); + printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); + fclose(fptr); + + if (!(self_test_status & GYRO_PASS_STATUS_BIT)) { + // Reset gyro bias data if gyro self-test failed + memset(gyro_bias,0, sizeof(gyro_bias)); + printf("Self-Test:Failed Gyro self-test\n"); + } + + if (self_test_status & ACCEL_PASS_STATUS_BIT) { + // Read Accel Bias + fptr= fopen(mpu.accl_bias, "r"); + if (fptr != NULL) { + fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i); + printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read accel bias\n"); + } + } else { + memset(accel_bias,0, sizeof(accel_bias)); + printf("Self-Test:Failed Accel self-test\n"); + } + + // Read temperature + if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT)) + { + fptr= fopen(mpu.temperature, "r"); + if (fptr != NULL) { + fscanf(fptr,"%d %ld", &temperature, ×tamp); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read temperature\n"); + } + } + + } else { + printf("Self-Test:ERR-Couldn't invoke self-test\n"); + } + + // When we read gyro bias, the bias is in raw units scaled by 1000. + // We store the bias in raw units scaled by 2^16 + save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f); + save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f); + save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f); + + // Save temperature @ time stored. Temperature is in degrees Celsius scaled by 2^16 + save_data.gyro_temp = temperature * (1L << 16); + 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 + 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; + } + + // Remove gravity, gravity in raw units should be 16384 for a 2g setting. + // We read data scaled by 1000, so + accel_bias[axis].l -= axis_sign * 4096L * 1000L; + + // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16 + save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f); + save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f); + save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f); + +#if 1 + printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0], + save_data.accel_bias[1], save_data.accel_bias[2]); + printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0], + save_data.gyro_bias[1], save_data.gyro_bias[2]); + printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp); + printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp); +#endif + + // 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"); + return -1; + } + + 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); + return result; +} + |