diff options
Diffstat (limited to '60xx/libsensors_iio/software')
56 files changed, 7614 insertions, 0 deletions
diff --git a/60xx/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk new file mode 100644 index 0000000..84e7e9b --- /dev/null +++ b/60xx/libsensors_iio/software/build/android/common.mk @@ -0,0 +1,87 @@ +# Use bash for additional echo fancyness +SHELL = /bin/bash + +#################################################################################################### +## defines + +# Build for Jellybean +BUILD_ANDROID_JELLYBEAN = 1 + +## libraries ## +LIB_PREFIX = lib + +STATIC_LIB_EXT = a +SHARED_LIB_EXT = so + +# normally, overridden from outside +# ?= assignment sets it only if not already defined +TARGET ?= android + +MLLITE_LIB_NAME ?= mllite +MPL_LIB_NAME ?= mplmpu + +## applications ## +SHARED_APP_SUFFIX = -shared +STATIC_APP_SUFFIX = -static + +#################################################################################################### +## compile, includes, and linker + +ifeq ($(BUILD_ANDROID_JELLYBEAN),1) +ANDROID_COMPILE = -DANDROID_JELLYBEAN=1 +endif + +ANDROID_LINK = -nostdlib +ANDROID_LINK += -fpic +ANDROID_LINK += -Wl,--gc-sections +ANDROID_LINK += -Wl,--no-whole-archive +ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib +ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +ANDROID_LINK_EXECUTABLE = $(ANDROID_LINK) +ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker +ifneq ($(BUILD_ANDROID_JELLYBEAN),1) +ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +endif +ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o +ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o + +ANDROID_INCLUDES = -I$(ANDROID_ROOT)/system/core/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include # ICS +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates + +KERNEL_INCLUDES = -I$(KERNEL_ROOT)/include + +INV_INCLUDES = -I$(INV_ROOT)/software/core/driver/include +INV_INCLUDES += -I$(MLLITE_DIR) +INV_INCLUDES += -I$(MLLITE_DIR)/linux + +INV_DEFINES += -DINV_CACHE_DMP=1 + +#################################################################################################### +## macros + +ifndef echo_in_colors +define echo_in_colors + echo -ne "\e[1;32m"$(1)"\e[0m" +endef +endif + + + diff --git a/60xx/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk new file mode 100644 index 0000000..47dedfe --- /dev/null +++ b/60xx/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/mpu_iio/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET) + +INSTALL_DIR = $(CURDIR) + +################################################################################ +## 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/60xx/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h new file mode 100644 index 0000000..c519691 --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/log.h @@ -0,0 +1,364 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/* + * This file incorporates work covered by the following copyright and + * permission notice: + * + * Copyright (C) 2005 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * C/C++ logging functions. See the logging documentation for API details. + * + * We'd like these to be available from C code (in case we import some from + * somewhere), so this has a C interface. + * + * The output will be correct when the log file is shared between multiple + * threads and/or multiple processes so long as the operating system + * supports O_APPEND. These calls have mutex-protected data structures + * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. + */ +#ifndef _LIBS_CUTILS_MPL_LOG_H +#define _LIBS_CUTILS_MPL_LOG_H + +#include <stdlib.h> +#include <stdarg.h> + +#ifdef ANDROID +#ifdef NDK_BUILD +#include "log_macros.h" +#else +#include <utils/Log.h> /* For the LOG macro */ +#endif +#endif + +#ifdef __KERNEL__ +#include <linux/kernel.h> +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + +/* --------------------------------------------------------------------- */ + +/* + * 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, ...) \ + ALOG(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/60xx/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h new file mode 100644 index 0000000..9725199 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h new file mode 100644 index 0000000..37194d6 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h new file mode 100644 index 0000000..12f2901 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h new file mode 100644 index 0000000..09eccce --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h new file mode 100644 index 0000000..b8c2511 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so Binary files differnew file mode 100755 index 0000000..9bdd5bc --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk new file mode 100644 index 0000000..1418450 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk @@ -0,0 +1,87 @@ +MLLITE_LIB_NAME = mllite +LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(INV_ROOT)/simple_apps/common +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -shared +LFLAGS += -Wl,-soname,$(LIBRARY) +LFLAGS += -Wl,-shared,-Bsymbolic +LFLAGS += $(ANDROID_LINK) +LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +#INV_SOURCES provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all mllite clean cleanall makefiles + +all: mllite + +mllite: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") + $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) + +$(OBJFOLDER) : + @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk new file mode 100644 index 0000000..011120c --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c new file mode 100644 index 0000000..0aa418d --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c @@ -0,0 +1,1228 @@ +/* + $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 <string.h> + +#include "ml_math_func.h" +#include "data_builder.h" +#include "mlmath.h" +#include "storage_manager.h" +#include "message_layer.h" +#include "results_holder.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "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]; + /** Sensor Accuracy */ + int gyro_accuracy; + int accel_accuracy; + int compass_accuracy; +}; + +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; + int mode; +#ifdef INV_PLAYBACK_DBG + int debug_mode; + int last_mode; + FILE *file; +#endif +}; + +void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); +static void inv_set_contiguous(void); + +static struct inv_data_builder_t inv_data_builder; +static struct inv_sensor_cal_t sensors; + +/** Change this key if the data being stored by this file changes */ +#define INV_DB_SAVE_KEY 53395 + +#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)); + // copy in the saved accuracy in the actual sensors accuracy + sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; + sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; + sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; + // TODO + if (sensors.compass.accuracy == 3) { + inv_set_compass_bias_found(1); + } + return INV_SUCCESS; +} + +/** 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; +} + +/** Sets the Orientation and Sensitivity of the gyro data. +* @param[in] orientation A scalar defining the transformation from chip mounting +* to the body frame. The function inv_orientation_matrix_to_scalar() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 +* such that degrees_per_second = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum rate * 2^15. +*/ +void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_G_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.gyro, orientation, + sensitivity); +} + +/** Set Gyro Sample rate in micro seconds. +* @param[in] sample_rate_us Set Gyro Sample rate in us +*/ +void inv_set_gyro_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.gyro.sample_rate_us = sample_rate_us; + sensors.gyro.sample_rate_ms = sample_rate_us / 1000; + if (sensors.gyro.bandwidth == 0) { + sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +/** Set Accel Sample rate in micro seconds. +* @param[in] sample_rate_us Set Accel Sample rate in us +*/ +void inv_set_accel_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.accel.sample_rate_us = sample_rate_us; + sensors.accel.sample_rate_ms = sample_rate_us / 1000; + if (sensors.accel.bandwidth == 0) { + sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +/** Pick the smallest non-negative number. Priority to td1 on equal +* If both are negative, return the largest. +*/ +static int inv_pick_best_time_difference(long td1, long td2) +{ + if (td1 >= 0) { + if (td2 >= 0) { + if (td1 <= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } else { + // td1 + return 0; + } + } else if (td2 >= 0) { + // td2 + return 1; + } else { + // Both are negative + if (td1 >= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } +} + +/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data. +*/ +static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts) +{ + int status = 0; + switch (sensor_number) { + case 0: // Quat + *ts = sensors.quat.timestamp; + if (inv_data_builder.mode & INV_QUAT_NEW) + if (sensors.quat.timestamp_prev != sensors.quat.timestamp) + status = 1; + return status; + case 1: // Gyro + *ts = sensors.gyro.timestamp; + if (inv_data_builder.mode & INV_GYRO_NEW) + if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp) + status = 1; + return status; + case 2: // Accel + *ts = sensors.accel.timestamp; + if (inv_data_builder.mode & INV_ACCEL_NEW) + if (sensors.accel.timestamp_prev != sensors.accel.timestamp) + status = 1; + return status; + case 3: // Compass + *ts = sensors.compass.timestamp; + if (inv_data_builder.mode & INV_MAG_NEW) + if (sensors.compass.timestamp_prev != sensors.compass.timestamp) + status = 1; + return status; + default: + *ts = 0; + return 0; + } + return 0; +} + +/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. +* It does this by finding a raw sensor that has the closest sample rate that is at least as +* often desired. It also returns if that raw sensor has a new piece of data. +* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel +* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. +*/ +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts) +{ + long td[2]; + int idx; + + if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { + // Sensor number is 0 (Quat) + return inv_raw_sensor_timestamp(0, ts); + } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { + return 0; // Accel must be on or 6-axis quat must be on + } + + // At this point, we know accel is on. Check if 3-axis quat is on + td[0] = sample_rate_us - sensors.quat.sample_rate_us; + td[1] = sample_rate_us - sensors.accel.sample_rate_us; + if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; + // 0 = quat, 3=accel + return inv_raw_sensor_timestamp(idx, ts); + } + + // No Quaternion data from outside, Gyro must be on + if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { + return 0; // Gyro must be on + } + + td[0] = sample_rate_us - sensors.gyro.sample_rate_us; + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; // 0=gyro 2=accel + idx++; + // 1 = gyro, 3=accel + return inv_raw_sensor_timestamp(idx, ts); +} + +/** Set Compass Sample rate in micro seconds. +* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. +*/ +void inv_set_compass_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.compass.sample_rate_us = sample_rate_us; + sensors.compass.sample_rate_ms = sample_rate_us / 1000; + if (sensors.compass.bandwidth == 0) { + sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.gyro.sample_rate_ms; +} + +void inv_get_accel_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.accel.sample_rate_ms; +} + +void inv_get_compass_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.compass.sample_rate_ms; +} + +/** Set Quat Sample rate in micro seconds. +* @param[in] sample_rate_us Set Quat Sample rate in us +*/ +void inv_set_quat_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.quat.sample_rate_us = sample_rate_us; + sensors.quat.sample_rate_ms = sample_rate_us / 1000; +} + +/** Set Gyro Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_gyro_bandwidth(int bandwidth_hz) +{ + sensors.gyro.bandwidth = bandwidth_hz; +} + +/** Set Accel Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_accel_bandwidth(int bandwidth_hz) +{ + sensors.accel.bandwidth = bandwidth_hz; +} + +/** Set Compass Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_compass_bandwidth(int bandwidth_hz) +{ + sensors.compass.bandwidth = bandwidth_hz; +} + +/** Helper function stating whether the compass is on or off. + * @return TRUE if compass if on, 0 if compass if off +*/ +int inv_get_compass_on() +{ + return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Helper function stating whether the gyro is on or off. + * @return TRUE if gyro if on, 0 if gyro if off +*/ +int inv_get_gyro_on() +{ + return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Helper function stating whether the acceleromter is on or off. + * @return TRUE if accel if on, 0 if accel if off +*/ +int inv_get_accel_on() +{ + return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Get last timestamp across all 3 sensors that are on. +* This find out which timestamp has the largest value for sensors that are on. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_time_t inv_get_last_timestamp() +{ + inv_time_t timestamp = 0; + if (sensors.accel.status & INV_SENSOR_ON) { + timestamp = sensors.accel.timestamp; + } + if (sensors.gyro.status & INV_SENSOR_ON) { + if (timestamp < sensors.gyro.timestamp) { + timestamp = sensors.gyro.timestamp; + } + } + 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. Also store raw data for body frame. +* @param[in,out] sensor structure to modify +* @param[in] bias bias in the mounting frame, in hardware units scaled by +* 2^16. Length 3. +*/ +void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) +{ + long raw32[3]; + + // Convert raw to calibrated + raw32[0] = (long)sensor->raw[0] << 15; + raw32[1] = (long)sensor->raw[1] << 15; + raw32[2] = (long)sensor->raw[2] << 15; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); + + raw32[0] -= bias[0] >> 1; + raw32[1] -= bias[1] >> 1; + raw32[2] -= bias[2] >> 1; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); + + sensor->status |= INV_CALIBRATED; +} + +/** Returns the current bias for the compass +* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. +* Length 3. +*/ +void inv_get_compass_bias(long *bias) +{ + if (bias != NULL) { + memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); + } +} + +void inv_set_compass_bias(const long *bias, int accuracy) +{ + if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { + memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); + inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); + } + sensors.compass.accuracy = accuracy; + inv_data_builder.save.compass_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); +} + +/** Set the state of a compass disturbance +* @param[in] dist 1=disturbance, 0=no disturbance +*/ +void inv_set_compass_disturbance(int dist) +{ + inv_data_builder.compass_disturbance = dist; +} + +int inv_get_compass_disturbance(void) { + return inv_data_builder.compass_disturbance; +} +/** Sets the 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_data_builder.save.accel_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + +/** Sets the accel accuracy. +* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. +*/ +void inv_set_accel_accuracy(int accuracy) +{ + sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + +/** 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; + inv_data_builder.save.accel_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + + +/** 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; + inv_data_builder.save.gyro_accuracy = accuracy; + + /* TODO: What should we do if there's no temperature data? */ + if (sensors.temp.calibrated[0]) + inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; + else + /* Set to 27 deg C for now until we've got a better solution. */ + inv_data_builder.save.gyro_temp = 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; + inv_data_builder.save.accel_accuracy = status & 3; + } + sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; + sensors.accel.timestamp_prev = sensors.accel.timestamp; + sensors.accel.timestamp = timestamp; + + return INV_SUCCESS; +} + +/** Record new gyro data and calls inv_execute_on_data() if previous +* sample has not been processed. +* @param[in] gyro Data is in device units. Length 3. +* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_GYRO; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); + sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.gyro.timestamp_prev = sensors.gyro.timestamp; + sensors.gyro.timestamp = timestamp; + inv_apply_calibration(&sensors.gyro, inv_data_builder.save.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; + inv_data_builder.save.compass_accuracy = status & 3; + } + sensors.compass.timestamp_prev = sensors.compass.timestamp; + sensors.compass.timestamp = timestamp; + sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; + + return INV_SUCCESS; +} + +/** Record new temperature data for use when inv_execute_on_data() is called. + * @param[in] temp Temperature data in q16 format. + * @param[in] timestamp Monotonic time stamp; for Android it's in + * nanoseconds. +* @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_TEMPERATURE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + sensors.temp.calibrated[0] = temp; + sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.temp.timestamp_prev = sensors.temp.timestamp; + sensors.temp.timestamp = timestamp; + /* TODO: Apply scale, remove offset. */ + + return INV_SUCCESS; +} +/** quaternion data +* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. +* Real part first. Length 4. +* @param[in] status number of axis, 16-bit or 32-bit +* @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; + +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_EXECUTE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + // Determine what new data we have + inv_data_builder.mode = 0; + if (sensors.gyro.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_GYRO_NEW; + if (sensors.accel.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_ACCEL_NEW; + if (sensors.compass.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_MAG_NEW; + if (sensors.temp.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_TEMP_NEW; + if (sensors.quat.status & INV_QUAT_NEW) + inv_data_builder.mode |= INV_QUAT_NEW; + + first_error = INV_SUCCESS; + + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) { + result = inv_data_builder.process[kk].func(&sensors); + if (result && !first_error) { + first_error = result; + } + } + } + + inv_set_contiguous(); + + return first_error; +} + +/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. +* +*/ +static void inv_set_contiguous(void) +{ + inv_time_t current_time = 0; + if (sensors.gyro.status & INV_NEW_DATA) { + sensors.gyro.status |= INV_CONTIGUOUS; + current_time = sensors.gyro.timestamp; + } + if (sensors.accel.status & INV_NEW_DATA) { + sensors.accel.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.accel.timestamp); + } + if (sensors.compass.status & INV_NEW_DATA) { + sensors.compass.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.compass.timestamp); + } + if (sensors.temp.status & INV_NEW_DATA) { + sensors.temp.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.temp.timestamp); + } + if (sensors.quat.status & INV_NEW_DATA) { + sensors.quat.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.quat.timestamp); + } + +#if 0 + /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() + * type functions. This is just in case that breaks down. We make sure + * all the data is within 2 seconds of the newest piece of data*/ + if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) + inv_gyro_was_turned_off(); + if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) + inv_accel_was_turned_off(); + if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) + inv_compass_was_turned_off(); + /* TODO: Temperature might not need to be read this quickly. */ + if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) + inv_temperature_was_turned_off(); +#endif + + /* clear bits */ + sensors.gyro.status &= ~INV_NEW_DATA; + sensors.accel.status &= ~INV_NEW_DATA; + sensors.compass.status &= ~INV_NEW_DATA; + sensors.temp.status &= ~INV_NEW_DATA; + sensors.quat.status &= ~INV_NEW_DATA; +} + +/** Gets a whole set of accel data including data, accuracy and timestamp. + * @param[out] data Accel Data where 1g = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + if (data != NULL) { + memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); + } + if (timestamp != NULL) { + *timestamp = sensors.accel.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.accel.accuracy; + } +} + +/** Gets a whole set of gyro data including data, accuracy and timestamp. + * @param[out] data Gyro Data where 1 dps = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); + if (timestamp != NULL) { + *timestamp = sensors.gyro.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.gyro.accuracy; + } +} + +/** Gets a whole set of gyro raw data including data, accuracy and timestamp. + * @param[out] data Gyro Data where 1 dps = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); + if (timestamp != NULL) { + *timestamp = sensors.gyro.timestamp; + } + if (accuracy != NULL) { + *accuracy = 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/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h new file mode 100644 index 0000000..c8c18cf --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h @@ -0,0 +1,240 @@ +/* + $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 quaternion is 6-axis from DMP */ +#define INV_QUAT_6AXIS 1024 +/** Set if quaternion is 3-axis from DMP */ +#define INV_QUAT_3AXIS 4096 + +/** 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_SHAKE 975 +#define INV_PRIORITY_SM 1000 + +struct inv_single_sensor_t { + /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when + * the rotation matrix could be thought of only having elements of 0,1,-1. + * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign. + * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row. + * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row. + * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row. + */ + int orientation; + /** The raw data in raw data units in the mounting frame */ + short raw[3]; + /** Raw data in body frame */ + long raw_scaled[3]; + /** Calibrated data */ + long calibrated[3]; + long sensitivity; + /** Sample rate in microseconds */ + long sample_rate_us; + long sample_rate_ms; + /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample + * skipped due to power savings turning off this sensor. + * INV_NEW_DATA set for a new set of data, cleared if not available. + * INV_CALIBRATED_SET if calibrated data has been solved for */ + int status; + /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */ + int accuracy; + inv_time_t timestamp; + inv_time_t timestamp_prev; + /** Bandwidth in Hz */ + int bandwidth; +}; +struct inv_quat_sensor_t { + long raw[4]; + /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample + * skipped due to power savings turning off this sensor. + * INV_NEW_DATA set for a new set of data, cleared if not available. + * INV_CALIBRATED_SET if calibrated data has been solved for */ + int status; + inv_time_t timestamp; + inv_time_t timestamp_prev; + long sample_rate_us; + long sample_rate_ms; +}; + +struct inv_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); + +void inv_get_gyro_sample_rate_ms(long *sample_rate_ms); +void inv_get_accel_sample_rate_ms(long *sample_rate_ms); +void inv_get_compass_sample_rate_ms(long *sample_rate_ms); + +inv_error_t inv_register_data_cb(inv_error_t (*func) + (struct inv_sensor_cal_t * data), int priority, + int sensor_type); +inv_error_t inv_unregister_data_cb(inv_error_t (*func) + (struct inv_sensor_cal_t * data)); + +inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp); +inv_error_t inv_build_compass(const long *compass, int status, + inv_time_t timestamp); +inv_error_t inv_build_accel(const long *accel, int status, + inv_time_t timestamp); +inv_error_t inv_build_temp(const long temp, inv_time_t timestamp); +inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp); +inv_error_t inv_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_accuracy(int accuracy); +void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); + +void inv_get_gyro_bias(long *bias, long *temp); +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_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); +void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp); + +void inv_get_gyro(long *gyro); + +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); + +// internal +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts); + +#ifdef __cplusplus +} +#endif + +#endif /* INV_DATA_BUILDER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c new file mode 100644 index 0000000..caa1db7 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c @@ -0,0 +1,497 @@ +/*
+ $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 <string.h>
+
+#include "hal_outputs.h"
+#include "log.h"
+#include "ml_math_func.h"
+#include "mlmath.h"
+#include "start_manager.h"
+#include "data_builder.h"
+#include "results_holder.h"
+
+struct hal_output_t {
+ int accuracy_mag; /**< Compass accuracy */
+// int accuracy_gyro; /**< Gyro Accuracy */
+// int accuracy_accel; /**< Accel Accuracy */
+ int accuracy_quat; /**< quat Accuracy */
+
+ inv_time_t nav_timestamp;
+ inv_time_t gam_timestamp;
+// inv_time_t accel_timestamp;
+ inv_time_t mag_timestamp;
+ long nav_quat[4];
+ int gyro_status;
+ int accel_status;
+ int compass_status;
+ int nine_axis_status;
+ inv_biquad_filter_t lp_filter[3];
+ float compass_float[3];
+ long linear_acceleration_sample_rate_us;
+ long gravity_sample_rate_us;
+};
+
+static struct hal_output_t hal_out;
+
+void inv_set_linear_acceleration_sample_rate(long sample_rate_us)
+{
+ hal_out.linear_acceleration_sample_rate_us = sample_rate_us;
+}
+
+void inv_set_gravity_sample_rate(long sample_rate_us)
+{
+ hal_out.gravity_sample_rate_us = sample_rate_us;
+}
+
+/** Acceleration (m/s^2) in body frame.
+* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
+* should return a vector of magnitude near 9.81 m/s^2
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_accel().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ int status;
+ /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
+ * So this 9.80665 / 2^16 */
+#define ACCEL_CONVERSION 0.000149637603759766f
+ long accel[3];
+ inv_get_accel_set(accel, accuracy, timestamp);
+ values[0] = accel[0] * ACCEL_CONVERSION;
+ values[1] = accel[1] * ACCEL_CONVERSION;
+ values[2] = accel[2] * ACCEL_CONVERSION;
+ if (hal_out.accel_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/** Linear Acceleration (m/s^2) in Body Frame.
+* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
+* accel biases while at rest.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_accel().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ long gravity[3], accel[3];
+ inv_time_t timestamp1;
+
+ inv_get_accel_set(accel, accuracy, ×tamp1);
+ inv_get_gravity(gravity);
+ accel[0] -= gravity[0] >> 14;
+ accel[1] -= gravity[1] >> 14;
+ accel[2] -= gravity[2] >> 14;
+ values[0] = accel[0] * ACCEL_CONVERSION;
+ values[1] = accel[1] * ACCEL_CONVERSION;
+ values[2] = accel[2] * ACCEL_CONVERSION;
+
+ return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp);
+}
+
+/** Gravity vector (m/s^2) in Body Frame.
+* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_accel().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ long gravity[3];
+
+ *accuracy = (int8_t) hal_out.accuracy_quat;
+ inv_get_gravity(gravity);
+ values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
+ values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
+ values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
+ return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp);
+}
+
+/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
+ * So this is: pi / 2^16 / 180 */
+#define GYRO_CONVERSION 2.66316109007924e-007f
+
+/** Gyroscope calibrated data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_gyro().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ long gyro[3];
+ int status;
+
+ inv_get_gyro_set(gyro, accuracy, timestamp);
+ values[0] = gyro[0] * GYRO_CONVERSION;
+ values[1] = gyro[1] * GYRO_CONVERSION;
+ values[2] = gyro[2] * GYRO_CONVERSION;
+ if (hal_out.gyro_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/** Gyroscope raw data (rad/s) in body frame.
+* @param[out] values Rotation Rate in rad/sec.
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
+* inv_build_gyro().
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ long gyro[3];
+ int status;
+
+ inv_get_gyro_set_raw(gyro, accuracy, timestamp);
+ values[0] = gyro[0] * GYRO_CONVERSION;
+ values[1] = gyro[1] * GYRO_CONVERSION;
+ values[2] = gyro[2] * GYRO_CONVERSION;
+ if (hal_out.gyro_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+/**
+* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
+* The rotation vector represents the orientation of the device as a combination
+* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
+* around an axis {x, y, z}. <br>
+* The three elements of the rotation vector are
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
+* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
+* equal to the direction of the axis of rotation.
+*
+* The three elements of the rotation vector are equal to the last three components of a unit quaternion
+* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
+*
+* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
+* The reference coordinate system is defined as a direct orthonormal basis, where:
+
+ -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
+ -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
+ -Z points towards the sky and is perpendicular to the ground.
+* @param[out] values Length 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 = (int8_t) hal_out.accuracy_quat;
+ *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
+ int i;
+
+ *timestamp = hal_out.mag_timestamp;
+ *accuracy = (int8_t) hal_out.accuracy_mag;
+
+ for (i=0; i<3; i++) {
+ values[i] = hal_out.compass_float[i];
+ }
+ if (hal_out.compass_status & INV_NEW_DATA)
+ status = 1;
+ else
+ status = 0;
+ return status;
+}
+
+static void inv_get_rotation(float r[3][3])
+{
+ long rot[9];
+ float conv = 1.f / (1L<<30);
+
+ inv_quaternion_to_rotation(hal_out.nav_quat, rot);
+ r[0][0] = rot[0]*conv;
+ r[0][1] = rot[1]*conv;
+ r[0][2] = rot[2]*conv;
+ r[1][0] = rot[3]*conv;
+ r[1][1] = rot[4]*conv;
+ r[1][2] = rot[5]*conv;
+ r[2][0] = rot[6]*conv;
+ r[2][1] = rot[7]*conv;
+ r[2][2] = rot[8]*conv;
+}
+
+static void google_orientation(float *g)
+{
+ float rad2deg = (float)(180.0 / M_PI);
+ float R[3][3];
+
+ inv_get_rotation(R);
+
+ g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
+ g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
+ g[2] = asinf ( R[2][0]) * rad2deg;
+ if (g[0] < 0)
+ g[0] += 360;
+}
+
+
+/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
+* @param[out] values Length 3, Degrees.<br>
+* - values[0]: Azimuth, angle between the magnetic north direction
+* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
+* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
+* when the z-axis moves toward the y-axis.<br>
+* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
+* values when the x-axis moves toward the z-axis.<br>
+*
+* @note This definition is different from yaw, pitch and roll used in aviation
+* where the X axis is along the long side of the plane (tail to nose).
+* Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
+* in conjunction with remapCoordinateSystem() and getOrientation() to compute
+* these values instead.
+* Important note: For historical reasons the roll angle is positive in the
+* clockwise direction (mathematically speaking, it should be positive in
+* the counter-clockwise direction).
+* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
+* @param[out] timestamp The timestamp for this sensor.
+* @return Returns 1 if the data was updated or 0 if it was not updated.
+*/
+int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
+ inv_time_t * timestamp)
+{
+ *accuracy = (int8_t) hal_out.accuracy_quat;
+ *timestamp = hal_out.nav_timestamp;
+
+ google_orientation(values);
+
+ 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 = 1000;
+ long compass[3];
+ int8_t accuracy;
+ int i;
+ (void) sensor_cal;
+
+ inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
+ &hal_out.nav_timestamp);
+ hal_out.gyro_status = sensor_cal->gyro.status;
+ hal_out.accel_status = sensor_cal->accel.status;
+ hal_out.compass_status = sensor_cal->compass.status;
+
+ // 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;
+ }
+
+ // Only output 9-axis if all 9 sensors are on.
+ if (sensor_cal->quat.status & INV_SENSOR_ON) {
+ // If quaternion sensor is on, gyros are not required as quaternion already has that part
+ if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+ use_sensor = -1;
+ }
+ } else {
+ if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
+ use_sensor = -1;
+ }
+ }
+
+ switch (use_sensor) {
+ case 0:
+ hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
+ break;
+ case 1:
+ hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->accel.timestamp;
+ break;
+ case 2:
+ hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->compass.timestamp;
+ break;
+ case 3:
+ hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
+ hal_out.nav_timestamp = sensor_cal->quat.timestamp;
+ break;
+ default:
+ hal_out.nine_axis_status = 0; // Don't output quaternion related info
+ break;
+ }
+
+ /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
+ * So this is: 1 / 2^16*/
+ #define COMPASS_CONVERSION 1.52587890625e-005f
+
+ inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
+ hal_out.accuracy_mag = (int ) accuracy;
+
+ for (i=0; i<3; i++) {
+ if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
+ INV_NEW_DATA ) {
+ // set the state variables to match output with input
+ inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);
+ }
+
+ if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
+ (INV_NEW_DATA | INV_RAW_DATA) ) {
+
+ hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],
+ (float ) compass[i]) * COMPASS_CONVERSION;
+
+ } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) {
+ hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;
+ }
+
+ }
+ return INV_SUCCESS;
+}
+
+/** 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;
+}
+/* file name: lowPassFilterCoeff_1_6.c */
+float compass_low_pass_filter_coeff[5] =
+{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
+
+/** Initializes hal outputs class. This is called automatically by the
+* enable function. It may be called any time the feature is enabled, but
+* is typically not needed to be called by outside callers.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_init_hal_outputs(void)
+{
+ int i;
+ memset(&hal_out, 0, sizeof(hal_out));
+ for (i=0; i<3; i++) {
+ inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
+ }
+
+ return INV_SUCCESS;
+}
+
+/** Turns on creation and storage of HAL type results.
+* @return Returns INV_SUCCESS if successful or an error code if not.
+*/
+inv_error_t inv_enable_hal_outputs(void)
+{
+ inv_error_t result;
+
+ // don't need to check the result for inv_init_hal_outputs
+ // since it's always INV_SUCCESS
+ inv_init_hal_outputs();
+
+ result = inv_register_mpl_start_notification(inv_start_hal_outputs);
+ return result;
+}
+
+/** Turns off creation and storage of HAL type results.
+*/
+inv_error_t inv_disable_hal_outputs(void)
+{
+ inv_error_t result;
+
+ inv_stop_hal_outputs(); // Ignore error if we have already stopped this
+ result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
+ return result;
+}
+
+/**
+ * @}
+ */
diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h new file mode 100644 index 0000000..41b72c6 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h @@ -0,0 +1,49 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_HAL_OUTPUTS_H__ +#define INV_HAL_OUTPUTS_H__ + +#ifdef __cplusplus +extern "C" { +#endif + + int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_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); + + // Set data rates for virtual sensors + void inv_set_linear_acceleration_sample_rate(long sample_rate_us); + void inv_set_gravity_sample_rate(long sample_rate_us); + +#ifdef __cplusplus +} +#endif + +#endif // INV_HAL_OUTPUTS_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h new file mode 100644 index 0000000..fb8e12b --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c new file mode 100644 index 0000000..72940f7 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -0,0 +1,279 @@ +/* + $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> + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-loaddmp" + +#include "ml_load_dmp.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 3065 + +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, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, + 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, + 0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, + 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, + 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, + 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, + 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, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, + 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, + 0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, + 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, + 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, + 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, + 0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, + /* bank # 11 */ + 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, + 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, + 0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, + 0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, + 0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, + 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, + 0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, + 0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, + 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, + 0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, + 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, + 0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, + 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, + 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, + 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, + 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff +}; + +#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/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h new file mode 100644 index 0000000..4d98692 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c new file mode 100644 index 0000000..b4c4249 --- /dev/null +++ b/60xx/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> + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-storeload" + +#include "ml_stored_data.h" +#include "storage_manager.h" +#include "mlos.h" + +#define LOADCAL_DEBUG 0 +#define STORECAL_DEBUG 0 + +#define DEFAULT_KEY 29681 + +#define STORECAL_LOG MPL_LOGI +#define LOADCAL_LOG MPL_LOGI + +inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) +{ + FILE *fp; + inv_error_t result = INV_SUCCESS; + size_t fsize; + + fp = fopen(MLCAL_FILE,"rb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + + // obtain file size + fseek (fp, 0 , SEEK_END); + fsize = ftell (fp); + rewind (fp); + + *calData = (unsigned char *)inv_malloc(fsize); + if (*calData==NULL) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", fsize); + fclose(fp); + return INV_ERROR_MEMORY_EXAUSTED; + } + + *bytesRead = fread(*calData, 1, fsize, fp); + if (*bytesRead != fsize) { + MPL_LOGE("bytes read (%d) don't match file size (%d)\n", + *bytesRead, fsize); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } + else { + MPL_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= NULL; + inv_error_t result = 0; + size_t bytesRead = 0; + + result = inv_read_cal(&calData, &bytesRead); + if(result != INV_SUCCESS) { + MPL_LOGE("Could not load cal file - " + "aborting\n"); + goto free_mem_n_exit; + } + + result = inv_load_mpl_states(calData, bytesRead); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not load the calibration data - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } + +free_mem_n_exit: + inv_free(calData); + return result; +} + +/** + * @brief Store runtime calibration data to a file + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_store_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + size_t length; + + result = inv_get_mpl_state_size(&length); + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + else { + MPL_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 result; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h new file mode 100644 index 0000000..115b34c --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h @@ -0,0 +1,53 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $ + * + ******************************************************************************/ + +#ifndef INV_MPL_STORED_DATA_H +#define INV_MPL_STORED_DATA_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Includes. +*/ +#include "mltypes.h" + +/* + Defines +*/ +#define MLCAL_FILE "/data/inv_cal_data.bin" + +/* + APIs +*/ +inv_error_t inv_load_calibration(void); +inv_error_t inv_store_calibration(void); + +/* + Internal APIs +*/ +inv_error_t inv_read_cal(unsigned char **, size_t *); +inv_error_t inv_write_cal(unsigned char *cal, size_t len); +inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len); +inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len); + +/* + Other prototypes +*/ +inv_error_t inv_load_cal(unsigned char *calData); +inv_error_t inv_store_cal(unsigned char *calData, size_t length); + +#ifdef __cplusplus +} +#endif +#endif /* INV_MPL_STORED_DATA_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c new file mode 100644 index 0000000..d0c4513 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c @@ -0,0 +1,423 @@ +#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" + +enum PROC_SYSFS_CMD { + CMD_GET_SYSFS_PATH, + CMD_GET_DMP_PATH, + CMD_GET_CHIP_NAME, + CMD_GET_SYSFS_KEY, + CMD_GET_TRIGGER_PATH, + CMD_GET_DEVICE_NODE +}; +static char sysfs_path[100]; +static char *chip_name[] = { + "ITG3500", + "MPU6050", + "MPU9150", + "MPU3050", + "MPU6500" +}; +static int chip_ind; +static int initialized =0; +static int status = 0; +static int iio_initialized = 0; +static int iio_dev_num = 0; + +#define IIO_MAX_NAME_LENGTH 30 + +#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" +#define FORMAT_TYPE_FILE "%s_type" + +#define CHIP_NUM ARRAY_SIZE(chip_name) + +static const char *iio_dir = "/sys/bus/iio/devices/"; + +/** + * find_type_by_name() - function to match top level types by name + * @name: top level type instance name + * @type: the type of top level instance being sort + * + * Typical types this is used for are device and trigger. + **/ +int find_type_by_name(const char *name, const char *type) +{ + const struct dirent *ent; + int number, numstrlen; + + FILE *nameFile; + DIR *dp; + char thisname[IIO_MAX_NAME_LENGTH]; + char *filename; + + dp = opendir(iio_dir); + if (dp == NULL) { + 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[4096], d; + char tmp[100]; + FILE *fp; + int i, j, result, find_flag; + int event_number = -1; + int input_number = -1; + + if(NULL == (fp = fopen(input, "rt")) ){ + return -1; + } + result = 1; + find_flag = 0; + while(result != 0 && find_flag < 2){ + i = 0; + d = 0; + memset(line, 0, 100); + while(d != '\n'){ + result = fread(&d, 1, 1, fp); + if(result == 0){ + line[0] = 0; + break; + } + sprintf(&line[i], "%c", d); + i ++; + } + if(line[0] == 'N'){ + i = 1; + while(line[i] != '"'){ + i++; + } + i++; + j = 0; + find_flag = 0; + if (mode == 0){ + while(j < CHIP_NUM){ + if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){ + find_flag = 1; + chip_ind = j; + } + j++; + } + } else if (mode != 0){ + if(!memcmp(&line[i], name, strlen(name))){ + find_flag = 1; + } + } + } + if(find_flag){ + if(mode == 0){ + if(line[0] == 'S'){ + memset(tmp, 0, 100); + i =1; + while(line[i] != '=') i++; + i++; + j = 0; + while(line[i] != '\n'){ + tmp[j] = line[i]; + i ++; j++; + } + sprintf(sysfs_path, "%s%s", "/sys", tmp); + find_flag++; + } + } else if(mode == 1){ + if(line[0] == 'H') { + i = 2; + while(line[i] != '=') i++; + while(line[i] != 't') i++; + i++; + event_number = 0; + while(line[i] != '\n'){ + if(line[i] >= '0' && line[i] <= '9') + event_number = event_number*10 + line[i]-0x30; + i ++; + } + find_flag ++; + } + } else if (mode == 2) { + if(line[0] == 'S'){ + memset(tmp, 0, 100); + i =1; + while(line[i] != '=') i++; + i++; + j = 0; + while(line[i] != '\n'){ + tmp[j] = line[i]; + i ++; j++; + } + input_number = 0; + if(tmp[j-2] >= '0' && tmp[j-2] <= '9') + input_number += (tmp[j-2]-0x30)*10; + if(tmp[j-1] >= '0' && tmp[j-1] <= '9') + input_number += (tmp[j-1]-0x30); + find_flag++; + } + } + } + } + fclose(fp); + if(find_flag == 0){ + return -1; + } + if(0 == mode) + status = 1; + if (mode == 1) + return event_number; + if (mode == 2) + return input_number; + return 0; + +} +static void init_iio() { + int i, j; + char iio_chip[10]; + int dev_num; + for(j=0; j< CHIP_NUM; j++) { + for (i=0; i<strlen(chip_name[j]); i++) { + iio_chip[i] = tolower(chip_name[j][i]); + } + iio_chip[strlen(chip_name[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(char *name) +{ + if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return iio device node. If iio is not initialized, return false. + * So the return must be checked to make sure the numeber is valid. + * @unsigned char *name: This should be array big enough to hold the device + * node. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_iio_device_node(char *name) +{ + if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h new file mode 100644 index 0000000..9470874 --- /dev/null +++ b/60xx/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(char *name); +inv_error_t inv_get_iio_device_node(char *name); + +#ifdef __cplusplus +} +#endif +#endif /* MLDMP_SYSFS_HELPER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h new file mode 100644 index 0000000..d4f8912 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h @@ -0,0 +1,99 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include <stdio.h> +#endif +#include <pthread.h> + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(LINUX) || defined(__KERNEL__) +typedef pthread_mutex_t* HANDLE; +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* - MLOSCreateFile defines. - */ + +#define MLOS_GENERIC_READ ((unsigned int)0x80000000) +#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) +#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) +#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) +#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) + + /* ---------- */ + /* - Enums. - */ + /* ---------- */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + +#ifndef __KERNEL__ +#include <string.h> + void *inv_malloc(unsigned int numBytes); + inv_error_t inv_free(void *ptr); + inv_error_t inv_create_mutex(HANDLE *mutex); + inv_error_t inv_lock_mutex(HANDLE mutex); + inv_error_t inv_unlock_mutex(HANDLE mutex); + FILE *inv_fopen(char *filename); + void inv_fclose(FILE *fp); + + inv_error_t inv_destroy_mutex(HANDLE handle); + + void inv_sleep(int mSecs); + unsigned long inv_get_tick_count(void); + + /* Kernel implmentations */ +#define GFP_KERNEL (0x70) + static inline void *kmalloc(size_t size, + unsigned int gfp_flags) + { + (void)gfp_flags; + return inv_malloc((unsigned int)size); + } + static inline void *kzalloc(size_t size, unsigned int gfp_flags) + { + void *tmp = inv_malloc((unsigned int)size); + (void)gfp_flags; + if (tmp) + memset(tmp, 0, size); + return tmp; + } + static inline void kfree(void *ptr) + { + inv_free(ptr); + } + static inline void msleep(long msecs) + { + inv_sleep(msecs); + } + static inline void udelay(unsigned long usecs) + { + inv_sleep((usecs + 999) / 1000); + } +#else +#include <linux/delay.h> +#endif + +#ifdef __cplusplus +} +#endif +#endif /* _MLOS_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c new file mode 100644 index 0000000..5424508 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c @@ -0,0 +1,190 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + ******************************************************************************/ + +/** + * @defgroup MLOS + * @brief OS Interface. + * + * @{ + * @file mlos.c + * @brief OS Interface. + */ + +/* ------------- */ +/* - Includes. - */ +/* ------------- */ + +#include <sys/time.h> +#include <unistd.h> +#include <pthread.h> +#include <stdlib.h> +#include <errno.h> + +#include "stdint_invensense.h" +#include "mlos.h" + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param num_bytes number of bytes + * @return pointer to allocated space + */ +void *inv_malloc(unsigned int num_bytes) +{ + // Allocate space. + void *alloc_ptr = malloc(num_bytes); + return alloc_ptr; +} + + +/** + * @brief Free allocated space + * @param ptr pointer to space to deallocate + * @return error code. + */ +inv_error_t inv_free(void *ptr) +{ + if (ptr) + free(ptr); + return INV_SUCCESS; +} + + +/** + * @brief Mutex create function + * @param mutex pointer to mutex handle + * @return error code. + */ +inv_error_t inv_create_mutex(HANDLE *mutex) +{ + int res; + pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); + if(pm == NULL) + return INV_ERROR; + + res = pthread_mutex_init(pm, NULL); + if(res == -1) { + free(pm); + return INV_ERROR_OS_CREATE_FAILED; + } + + *mutex = (HANDLE)pm; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex lock function + * @param mutex Mutex handle + * @return error code. + */ +inv_error_t inv_lock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t *)mutex; + + res = pthread_mutex_lock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex unlock function + * @param mutex mutex handle + * @return error code. + */ +inv_error_t inv_unlock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t *)mutex; + + res = pthread_mutex_unlock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief open file + * @param filename name of the file to open. + * @return error code. + */ +FILE *inv_fopen(char *filename) +{ + FILE *fp = fopen(filename, "r"); + return fp; +} + + +/** + * @brief close the file. + * @param fp handle to file to close. + * @return error code. + */ +void inv_fclose(FILE *fp) +{ + fclose(fp); +} + +/** + * @brief Close Handle + * @param handle handle to the resource. + * @return Zero if success, an error code otherwise. + */ +inv_error_t inv_destroy_mutex(HANDLE handle) +{ + int error; + pthread_mutex_t *pm = (pthread_mutex_t *)handle; + error = pthread_mutex_destroy(pm); + if (error) + return errno; + free((void*) handle); + + return INV_SUCCESS;} + + +/** + * @brief Sleep function. + */ +void inv_sleep(int m_secs) +{ + usleep(m_secs * 1000); +} + + +/** + * @brief get system's internal tick count. + * Used for time reference. + * @return current tick count. + */ +unsigned long inv_get_tick_count() +{ + struct timeval tv; + + if (gettimeofday(&tv, NULL) != 0) + return 0; + + return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); +} + +/** @} */ + diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c new file mode 100644 index 0000000..8317957 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h new file mode 100644 index 0000000..df0c0e2 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/ml_math_func.c b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c new file mode 100644 index 0000000..c30d693 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c @@ -0,0 +1,706 @@ +/* + $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] x a vector [3x1] +* @return the normalize vector. +*/ +double inv_vector_norm(const float *x) +{ + return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); +} + +void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { + int i; + // initial state to zero + pFilter->state[0] = 0; + pFilter->state[1] = 0; + + // set up coefficients + for (i=0; i<5; i++) { + pFilter->c[i] = pBiquadCoeff[i]; + } +} + +void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) +{ + pFilter->input = input; + pFilter->output = input; + pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); + pFilter->state[1] = pFilter->state[0]; +} + +float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { + float stateZero; + + pFilter->input = input; + // calculate the new state; + stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] + - pFilter->c[3]*pFilter->state[1]; + + pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] + + pFilter->c[1]*pFilter->state[1]; + + // update the output and state + pFilter->output = pFilter->output * pFilter->c[4]; + pFilter->state[1] = pFilter->state[0]; + pFilter->state[0] = stateZero; + return pFilter->output; +} + +void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { + + cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; + cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; + cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h new file mode 100644 index 0000000..535d849 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h @@ -0,0 +1,120 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INVENSENSE_INV_MATH_FUNC_H__ +#define INVENSENSE_INV_MATH_FUNC_H__ + +#include "mltypes.h" + +#define GYRO_MAG_SQR_SHIFT 6 +#define NUM_ROTATION_MATRIX_ELEMENTS (9) +#define ROT_MATRIX_SCALE_LONG (1073741824L) +#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) +#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ + ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) +#define SIGNM(k)((int)(k)&1?-1:1) +#define SIGNSET(x) ((x) ? -1 : +1) + +#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f + +#ifdef __cplusplus +extern "C" { +#endif + + typedef struct { + float state[4]; + float c[5]; + float input; + float output; + } inv_biquad_filter_t; + + static inline float inv_q30_to_float(long q30) + { + return (float) q30 / ((float)(1L << 30)); + } + + static inline double inv_q30_to_double(long q30) + { + return (double) q30 / ((double)(1L << 30)); + } + + static inline float inv_q16_to_float(long q16) + { + return (float) q16 / (1L << 16); + } + + static inline double inv_q16_to_double(long q16) + { + return (double) q16 / (1L << 16); + } + + + + + long inv_q29_mult(long a, long b); + long inv_q30_mult(long a, long b); + + /* UMPL_ELIMINATE_64BIT Notes: + * An alternate implementation using float instead of long long accudoublemulators + * is provided for q29_mult and q30_mult. + * When long long accumulators are used and an alternate implementation is not + * available, we eliminate the entire function and header with a macro. + */ +#ifndef UMPL_ELIMINATE_64BIT + long inv_q30_div(long a, long b); + long inv_q_shift_mult(long a, long b, int shift); +#endif + + void inv_q_mult(const long *q1, const long *q2, long *qProd); + void inv_q_add(long *q1, long *q2, long *qSum); + void inv_q_normalize(long *q); + void inv_q_invert(const long *q, long *qInverted); + void inv_q_multf(const float *q1, const float *q2, float *qProd); + void inv_q_addf(const float *q1, const float *q2, float *qSum); + void inv_q_normalizef(float *q); + void inv_q_norm4(float *q); + void inv_q_invertf(const float *q, float *qInverted); + void inv_quaternion_to_rotation(const long *quat, long *rot); + unsigned char *inv_int32_to_big8(long x, unsigned char *big8); + long inv_big8_to_int32(const unsigned char *big8); + short inv_big8_to_int16(const unsigned char *big8); + short inv_little8_to_int16(const unsigned char *little8); + unsigned char *inv_int16_to_big8(short x, unsigned char *big8); + float inv_matrix_det(float *p, int *n); + void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); + double inv_matrix_detd(double *p, int *n); + void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); + float inv_wrap_angle(float ang); + float inv_angle_diff(float ang1, float ang2); + void inv_quaternion_to_rotation_vector(const long *quat, long *rot); + unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); + void inv_convert_to_body(unsigned short orientation, const long *input, long *output); + void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); + void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); + void inv_q_rotate(const long *q, const long *in, long *out); + void inv_vector_normalize(long *vec, int length); + uint32_t inv_checksum(const unsigned char *str, int len); + float inv_compass_angle(const long *compass, const long *grav, + const float *quat); + unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); + + static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) + { + return (long)((t1 - t2) / 1000000L); + } + + double quaternion_to_rotation_angle(const long *quat); + double inv_vector_norm(const float *x); + + void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); + float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); + void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); + void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); + +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.c b/60xx/libsensors_iio/software/core/mllite/mpl.c new file mode 100644 index 0000000..9141cc6 --- /dev/null +++ b/60xx/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.1.2 beta RC9";
+
+/**
+ * @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/60xx/libsensors_iio/software/core/mllite/mpl.h b/60xx/libsensors_iio/software/core/mllite/mpl.h new file mode 100644 index 0000000..a6b5ac7 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/results_holder.c b/60xx/libsensors_iio/software/core/mllite/results_holder.c new file mode 100644 index 0000000..df58f40 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/results_holder.c @@ -0,0 +1,522 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @defgroup Results_Holder results_holder + * @brief Motion Library - Results Holder + * Holds the data for MPL + * + * @{ + * @file results_holder.c + * @brief Results Holder for HAL. + */ + +#include <string.h> + +#include "results_holder.h" +#include "ml_math_func.h" +#include "mlmath.h" +#include "start_manager.h" +#include "data_builder.h" +#include "message_layer.h" +#include "log.h" + +// 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 */ + int got_accel_bias; /**< Flag describing if accel bias is known */ + long compass_bias_error[3]; /**< Error Squared */ + unsigned char motion_state; + unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ + long compass_count; /**< compass state internal counter */ + int got_compass_bias; /**< Flag describing if compass bias is known */ + int large_mag_field; /**< Flag describing if there is a large magnetic field */ + int compass_state; /**< Internal compass state */ + long status; + struct inv_sensor_cal_t *sensor; + float quat_confidence_interval; +}; +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 accel bias. + * @return return 1 if we know the accel bias, 0 if not. + * it is set with inv_set_accel_bias_found() + */ +int inv_got_accel_bias() +{ + return rh.got_accel_bias; +} + +/** Sets whether we know the accel bias + * @param[in] state Set to 1 if we know the accel bias. + * Can be retrieved with inv_got_accel_bias() + */ +void inv_set_accel_bias_found(int state) +{ + rh.got_accel_bias = state; +} + +/** Sets state of if we know the compass bias. + * @return return 1 if we know the compass bias, 0 if not. + * it is set with inv_set_compass_bias_found() + */ +int inv_got_compass_bias() +{ + return rh.got_compass_bias; +} + +/** Sets whether we know the compass bias + * @param[in] state Set to 1 if we know the compass bias. + * Can be retrieved with inv_got_compass_bias() + */ +void inv_set_compass_bias_found(int state) +{ + rh.got_compass_bias = state; +} + +/** Sets the compass state. + * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). + */ +void inv_set_compass_state(int state) +{ + rh.compass_state = state; +} + +/** Get's the compass state + * @return the compass state that was set with inv_set_compass_state() + */ +int inv_get_compass_state() +{ + return rh.compass_state; +} + +/** Set compass bias error. See inv_get_compass_bias_error() + * @param[in] bias_error Set's how accurate we know the compass bias. It is the + * error squared. + */ +void inv_set_compass_bias_error(const long *bias_error) +{ + memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); +} + +/** Get's compass bias error. See inv_set_compass_bias_error() for setting. + * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. + */ +void inv_get_compass_bias_error(long *bias_error) +{ + memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame + * with gravity removed + * @param[out] data 3-element vector of accelerometer data in body frame + * with gravity removed + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_linear_accel(long *data) +{ + long gravity[3]; + + if (data != NULL) + { + inv_get_accel_set(data, NULL, NULL); + inv_get_gravity(gravity); + data[0] -= gravity[0] >> 14; + data[1] -= gravity[1] >> 14; + data[2] -= gravity[2] >> 14; + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame + * @param[out] data 3-element vector of accelerometer data in body frame + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_accel(long *data) +{ + if (data != NULL) { + inv_get_accel_set(data, NULL, NULL); + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @brief Returns 3-element vector of accelerometer float data + * @param[out] data 3-element vector of accelerometer float data + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_accel_float(float *data) +{ + long tdata[3]; + unsigned char i; + + if (data != NULL && !inv_get_accel(tdata)) { + for (i = 0; i < 3; ++i) { + data[i] = ((float)tdata[i] / (1L << 16)); + } + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @brief Returns 3-element vector of gyro float data + * @param[out] data 3-element vector of gyro float data + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_gyro_float(float *data) +{ + long tdata[3]; + unsigned char i; + + if (data != NULL) { + inv_get_gyro_set(tdata, NULL, NULL); + for (i = 0; i < 3; ++i) { + data[i] = ((float)tdata[i] / (1L << 16)); + } + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** Set 9 axis 95% heading confidence interval for quaternion +* @param[in] ci Confidence interval in radians. +*/ +void inv_set_heading_confidence_interval(float ci) +{ + rh.quat_confidence_interval = ci; +} + +/** Get 9 axis 95% heading confidence interval for quaternion +* @return Confidence interval in radians. +*/ +float inv_get_heading_confidence_interval(void) +{ + return rh.quat_confidence_interval; +} + +/** + * @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/60xx/libsensors_iio/software/core/mllite/results_holder.h b/60xx/libsensors_iio/software/core/mllite/results_holder.h new file mode 100644 index 0000000..09da24e --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/results_holder.h @@ -0,0 +1,81 @@ +/* + $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); + +int inv_got_accel_bias(); +void inv_set_accel_bias_found(int state); + + +#ifdef __cplusplus +} +#endif + +#endif // INV_RESULTS_HOLDER_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.c b/60xx/libsensors_iio/software/core/mllite/start_manager.c new file mode 100644 index 0000000..fb758e7 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/start_manager.h b/60xx/libsensors_iio/software/core/mllite/start_manager.h new file mode 100644 index 0000000..899e3f5 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c new file mode 100644 index 0000000..3e4e619 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.c @@ -0,0 +1,204 @@ +/* + $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 <string.h> + +#include "storage_manager.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" + +/* Must be changed if the format of storage changes */ +#define DEFAULT_KEY 29681 + +typedef inv_error_t (*load_func_t)(const unsigned char *data); +typedef inv_error_t (*save_func_t)(unsigned char *data); +/** Max number of entites that can be stored */ +#define NUM_STORAGE_BOXES 20 + +struct data_header_t { + long size; + uint32_t checksum; + unsigned int key; +}; + +struct data_storage_t { + int num; /**< Number of differnt save entities */ + size_t total_size; /**< Size in bytes to store non volatile data */ + load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ + save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ + struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ +}; +static struct data_storage_t ds; + +/** Should be called once before using any of the storage methods. Typically +* called first by inv_init_mpl().*/ +void inv_init_storage_manager() +{ + memset(&ds, 0, sizeof(ds)); + ds.total_size = sizeof(struct data_header_t); +} + +/** Used to register your mechanism to load and store non-volative data. This should typical be +* called during the enable function for your feature. +* @param[in] load_func function pointer you will use to receive data that was stored for you. +* @param[in] save_func function pointer you will use to save any data you want saved to +* non-volatile memory between runs. +* @param[in] size The size in bytes of the amount of data you want loaded and saved. +* @param[in] key The key associated with your data type should be unique across MPL. +* The key should change when your type of data for storage changes. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), + inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) +{ + int kk; + // Check if this has been registered already + for (kk=0; kk<ds.num; ++kk) { + if (key == ds.hd[kk].key) { + return INV_ERROR_INVALID_PARAMETER; + } + } + // Make sure there is room + if (ds.num >= NUM_STORAGE_BOXES) { + return INV_ERROR_INVALID_PARAMETER; + } + // Add to list + ds.hd[ds.num].key = key; + ds.hd[ds.num].size = size; + ds.load[ds.num] = load_func; + ds.save[ds.num] = save_func; + ds.total_size += size + sizeof(struct data_header_t); + ds.num++; + + return INV_SUCCESS; +} + +/** Returns the memory size needed to perform a store +* @param[out] size Size in bytes of memory needed to store. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_get_mpl_state_size(size_t *size) +{ + *size = ds.total_size; + return INV_SUCCESS; +} + +/** @internal + * Finds key in ds.hd[] array and returns location + * @return location where key exists in array, -1 if not found. + */ +static int inv_find_entry(unsigned int key) +{ + int kk; + for (kk=0; kk<ds.num; ++kk) { + if (key == ds.hd[kk].key) { + return kk; + } + } + return -1; +} + +/** This function takes a block of data that has been saved in non-volatile memory and pushes +* to the proper locations. Multiple error checks are performed on the data. +* @param[in] data Data that was saved to be loaded up by MPL +* @param[in] length Length of data vector in bytes +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) +{ + struct data_header_t *hd; + int entry; + uint32_t checksum; + long len; + + len = length; // Important so we get negative numbers + if (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/60xx/libsensors_iio/software/core/mllite/storage_manager.h b/60xx/libsensors_iio/software/core/mllite/storage_manager.h new file mode 100644 index 0000000..7255591 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h new file mode 100644 index 0000000..5a53213 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/authenticate.h b/60xx/libsensors_iio/software/core/mpl/authenticate.h new file mode 100644 index 0000000..d7c803b --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so Binary files differnew file mode 100644 index 0000000..fbd346f --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk new file mode 100644 index 0000000..2e15205 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk @@ -0,0 +1,88 @@ +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 += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -shared +LFLAGS += -Wl,-soname,$(LIBRARY) +LFLAGS += -Wl,-shared,-Bsymbolic +LFLAGS += $(ANDROID_LINK) +LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES, VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all mpl clean cleanall + +all: mpl + +mpl: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) + @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") + $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) + +$(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/60xx/libsensors_iio/software/core/mpl/build/filelist.mk b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk new file mode 100644 index 0000000..e18003a --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h new file mode 100644 index 0000000..4f01fc2 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/compass_fit.h b/60xx/libsensors_iio/software/core/mpl/compass_fit.h new file mode 100644 index 0000000..be3cce8 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h new file mode 100644 index 0000000..a3e044c --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h new file mode 100644 index 0000000..c553926 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h @@ -0,0 +1,46 @@ +/*
+ $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, long long *param);
+ void inv_get_fast_nomot_compass_param(long *cntr, long long *param);
+ void inv_set_fast_nomot_accel_threshold(long long thresh);
+ void inv_set_fast_nomot_compass_threshold(long long thresh);
+ void int_set_fast_nomot_gyro_threshold(long long thresh);
+
+ void inv_fnm_debug_print(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // MLDMP_FAST_NO_MOTION_H__
+
diff --git a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h new file mode 100644 index 0000000..1ba1ebb --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h @@ -0,0 +1,37 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_FUSION9AXIS_H__ +#define MLDMP_FUSION9AXIS_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + void inv_init_9x_fusion(void); + inv_error_t inv_9x_fusion_state_change(unsigned char newState); + inv_error_t inv_enable_9x_sensor_fusion(void); + inv_error_t inv_disable_9x_sensor_fusion(void); + inv_error_t inv_start_9x_sensor_fusion(void); + inv_error_t inv_stop_9x_sensor_fusion(void); + inv_error_t inv_9x_fusion_set_mag_fb(double fb);
+ inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);
+ +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FUSION9AXIS_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h new file mode 100644 index 0000000..69918d5 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h new file mode 100644 index 0000000..09ecc42 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h new file mode 100644 index 0000000..6620bbf --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/invensense_adv.h b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h new file mode 100644 index 0000000..12932c9 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h @@ -0,0 +1,31 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/* + Main header file for Invensense's Advanced library. +*/ + +#include "accel_auto_cal.h" +#include "compass_bias_w_gyro.h" +#include "compass_fit.h" +#include "compass_vec_cal.h" +#include "fast_no_motion.h" +#include "fusion_9axis.h" +#include "gyro_tc.h" +#include "heading_from_gyro.h" +#include "mag_disturb.h" +#include "motion_no_motion.h" +#include "no_gyro_fusion.h" +#include "quaternion_supervisor.h" +#include "mag_disturb.h" +#include "quat_accuracy_monitor.h" +#include "shake.h" diff --git a/60xx/libsensors_iio/software/core/mpl/mag_disturb.h b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h new file mode 100644 index 0000000..38df919 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h new file mode 100644 index 0000000..01cf1c0 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h new file mode 100644 index 0000000..38d5690 --- /dev/null +++ b/60xx/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/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h new file mode 100644 index 0000000..3c6a2c1 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h @@ -0,0 +1,71 @@ +/*
+ quat_accuracy_monitor.h
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+
+/*******************************************************************************
+ *
+ * $Id:$
+ *
+ ******************************************************************************/
+
+#ifndef QUAT_ACCURARCY_MONITOR_H__
+#define QUAT_ACCURARCY_MONITOR_H__
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+enum accuracy_signal_type_e {
+ TYPE_NAV_QUAT,
+ TYPE_GAM_QUAT,
+ TYPE_NAV_QUAT_ADVANCED,
+ TYPE_GAM_QUAT_ADVANCED,
+ TYPE_NAV_QUAT_BASIC,
+ TYPE_GAM_QUAT_BASIC,
+ TYPE_MAG,
+ TYPE_GYRO,
+ TYPE_ACCEL,
+};
+
+inv_error_t inv_init_quat_accuracy_monitor(void);
+
+void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
+double get_accuracy_threshold(enum accuracy_signal_type_e type);
+void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
+int get_accuracy_weight(enum accuracy_signal_type_e type);
+
+int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
+
+void inv_reset_quat_accuracy(void);
+double get_6axis_correction_term(void);
+double get_9axis_correction_term(void);
+int get_9axis_accuracy_state();
+
+void set_6axis_error_average(double value);
+double get_6axis_error_bound(void);
+double get_compass_correction(void);
+double get_9axis_error_bound(void);
+
+float get_confidence_interval(void);
+void set_compass_uncertainty(float value);
+
+inv_error_t inv_enable_quat_accuracy_monitor(void);
+inv_error_t inv_disable_quat_accuracy_monitor(void);
+inv_error_t inv_start_quat_accuracy_monitor(void);
+inv_error_t inv_stop_quat_accuracy_monitor(void);
+
+double get_compassNgravity(void);
+double get_init_compassNgravity(void);
+
+float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h new file mode 100644 index 0000000..4fa36b0 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h @@ -0,0 +1,27 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_QUATERNION_SUPERVISOR_H__
+#define INV_QUATERNION_SUPERVISOR_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+inv_error_t inv_enable_quaternion(void);
+inv_error_t inv_disable_quaternion(void);
+inv_error_t inv_init_quaternion(void);
+inv_error_t inv_start_quaternion(void);
+void inv_set_quaternion(long *quat); +
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h new file mode 100644 index 0000000..67acb7b --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/shake.h @@ -0,0 +1,94 @@ +/*
+ $License:
+ Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
+ See included License.txt for License information.
+ $
+ */
+#ifndef INV_SHAKE_H__
+#define INV_SHAKE_H__
+
+#include "mltypes.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /* ------------ */
+ /* - Defines. - */
+ /* ------------ */
+
+ #define STATE_ZERO 0
+ #define STATE_INIT_1 1
+ #define STATE_INIT_2 2
+ #define STATE_DETECT 3
+
+ struct t_shake_config_params {
+ long shake_time_min_ms;
+ long shake_time_max_ms;
+ long shake_time_min;
+ long shake_time_max;
+ unsigned char shake_time_set;
+ long shake_time_saved;
+ float shake_deriv_thr;
+ int zero_cross_thr;
+ float accel_delta_min;
+ float accel_delta_max;
+ unsigned char interp_enable;
+ };
+
+ struct t_shake_state_params {
+ unsigned char state;
+ float accel_peak_high;
+ float accel_peak_low;
+ float accel_range;
+ int num_zero_cross;
+ short curr_shake_time;
+ int deriv_major_change;
+ int deriv_major_sign;
+ float accel_buffer[200];
+ float delta_buffer[200];
+ };
+
+ struct t_shake_data_params {
+ float accel_prev;
+ float accel_curr;
+ float delta_prev;
+ float delta_curr;
+ float delta_prev_buffer;
+ };
+
+ struct t_shake_results {
+ //unsigned char shake_int;
+ int shake_number;
+ };
+
+ struct t_shake_cb {
+ void (*shake_callback)(struct t_shake_results *shake_results);
+ };
+
+
+ /* --------------------- */
+ /* - Function p-types. - */
+ /* --------------------- */
+ inv_error_t inv_enable_shake(void);
+ inv_error_t inv_disable_shake(void);
+ inv_error_t inv_init_shake(void);
+ inv_error_t inv_start_shake(void);
+ int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
+ void inv_config_shake_time_params(long sample_time_ms);
+ void inv_set_shake_accel_delta_min(float accel_g);
+ void inv_set_shake_accel_delta_max(float accel_g);
+ void inv_set_shake_zero_cross_thresh(int num_zero_cross);
+ void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
+ void inv_set_shake_time_min_ms(long time_ms);
+ void inv_set_shake_time_max_ms(long time_ms);
+ void inv_enable_shake_data_interpolation(unsigned char en);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // INV_SHAKE__
\ No newline at end of file |