diff options
Diffstat (limited to 'libsensors/software/core/mllite')
34 files changed, 0 insertions, 6148 deletions
diff --git a/libsensors/software/core/mllite/CMakeLists.txt b/libsensors/software/core/mllite/CMakeLists.txt deleted file mode 100644 index 8650553..0000000 --- a/libsensors/software/core/mllite/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -## CMakeLists for mlsdk/mldmp - -project(mldmp C) - -if (NOT CMAKE_BUILD_ENGINEERING) - -# # just copy the library from mldmp/mpl/$PLATFORM to mldmp/ -# if (CMAKE_SYSTEM_NAME MATCHES Windows) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Android) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Linux) -# message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a") -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Catalina) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# endif() -# # better way that doesn't work for now -# # add_custom_command( -# # TARGET mpl -# # PRE_BUILD -# # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR} -# # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}" -# # ) - -else (NOT CMAKE_BUILD_ENGINEERING) - - set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) - include(Engineering) - -endif (NOT CMAKE_BUILD_ENGINEERING) diff --git a/libsensors/software/core/mllite/Engineering.cmake b/libsensors/software/core/mllite/Engineering.cmake deleted file mode 100644 index 42f2429..0000000 --- a/libsensors/software/core/mllite/Engineering.cmake +++ /dev/null @@ -1,150 +0,0 @@ -## Engineering CMakeLists for software/core/mllite - -include_directories( - . - ${SOLUTION_SOURCE_DIR}/core/mpl - ${SOLUTION_SOURCE_DIR}/core/HAL - ${SOLUTION_SOURCE_DIR}/driver/include -) - -add_definitions ( - -DINV_INCLUDE_LEGACY_HEADERS -) - -if (CMAKE_SYSTEM_NAME MATCHES Android) - - include_directories( - ${SOLUTION_SOURCE_DIR}/core/mllite/linux - ${SOLUTION_SOURCE_DIR}/driver/include/linux - ) - add_definitions( - -DLINUX - -D_REENTRANT - ) - set (HEADERS - ${HEADERS} - linux/mlos.h - linux/ml_stored_data.h - linux/ml_load_dmp.h - linux/ml_sysfs_helper.h - ) - set (SOURCES - ${SOURCES} - linux/mlos_linux.c - linux/ml_stored_data.c - linux/ml_load_dmp.c - linux/ml_sysfs_helper.c - ) - -elseif (CMAKE_SYSTEM_NAME MATCHES Linux) - - add_definitions( - -DLINUX - -D_REENTRANT - ) - set (HEADERS - ${HEADERS} - ) - set (SOURCES - ${SOURCES} - ) - -elseif (CMAKE_SYSTEM_NAME MATCHES Windows) - - add_definitions( - -DAIO - -DUART_COM - -D_CRT_SECURE_NO_WARNINGS - -D_CRT_SECURE_NO_DEPRECATE - ) - set (HEADERS - ${HEADERS} - ) - set (SOURCES - ${SOURCES} - ) - -endif () - -set (HEADERS - ${HEADERS} - data_builder.h - hal_outputs.h - message_layer.h - ml_math_func.h - mpl.h - results_holder.h - start_manager.h - storage_manager.h -) -set (SOURCES - ${SOURCES} - data_builder.c - hal_outputs.c - message_layer.c - ml_math_func.c - mpl.c - results_holder.c - start_manager.c - storage_manager.c -) - -# coveniently add this file to the resources for easy access within IDEs -set (RESOURCES - Engineering.cmake -) - -if (CMAKE_TESTING_SUPPORT) - - message("Including Testing support") - include_directories( - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport - ) - add_definitions( - -DSELF_VERIFICATION - ) - set( - HEADERS - ${HEADERS} - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h - ) - set( - SOURCES - ${SOURCES} - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c - ) - -endif () - -############ -## TARGET ## -############ -add_library( - mllite STATIC - ${SOURCES} - ${HEADERS} - ${RESOURCES} -) -set_target_properties( - mllite - PROPERTIES CLEAN_DIRECT_OUTPUT 1 -) - -if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)") - - add_library( - mllite_shared SHARED - ${SOURCES} - ${HEADERS} - ${RESOURCES} - ) - FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite") - - install ( - TARGETS mllite_shared - DESTINATION lib - ) - -endif () - - diff --git a/libsensors/software/core/mllite/build/android/shared.mk b/libsensors/software/core/mllite/build/android/shared.mk deleted file mode 100644 index 2ee2e20..0000000 --- a/libsensors/software/core/mllite/build/android/shared.mk +++ /dev/null @@ -1,91 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- -COMP ?= $(CROSS)gcc -LINK ?= $(CROSS)gcc - -OBJFOLDER = $(CURDIR)/obj - -INV_ROOT = ../../../../.. -MLLITE_DIR = $(INV_ROOT)/software/core/mllite - -include $(INV_ROOT)/software/build/android/common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += -Wall -CFLAGS += -fpic -CFLAGS += -nostdlib -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -CFLAGS += -DLINUX -CFLAGS += -DANDROID -CFLAGS += -mthumb-interwork -CFLAGS += -fno-exceptions -CFLAGS += -ffunction-sections -CFLAGS += -funwind-tables -CFLAGS += -fstack-protector -CFLAGS += -fno-short-enums -CFLAGS += -fmessage-length=0 -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(INV_ROOT)/simple_apps/common -CFLAGS += $(INV_INCLUDES) -CFLAGS += $(INV_DEFINES) - -LLINK = -lc -LLINK += -lm -LLINK += -lutils -LLINK += -lcutils -LLINK += -lgcc -LLINK += -ldl - -LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += -shared -LFLAGS += -Wl,-soname,$(LIBRARY) -LFLAGS += -nostdlib -LFLAGS += -fpic -LFLAGS += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.xsc -LFLAGS += -Wl,--gc-sections -LFLAGS += -Wl,--no-whole-archive -LFLAGS += -Wl,-shared,-Bsymbolic -LFLAGS += $(ANDROID_LINK) -LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib - -#################################################################################################### -## sources - -#INV_SOURCES provided by Makefile.filelist -include ../filelist.mk - -INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) -INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all mllite clean cleanall makefiles - -all: mllite - -mllite: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) - @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") - $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) - -$(OBJFOLDER) : - @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") - mkdir obj - -$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") - $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/libsensors/software/core/mllite/build/android/static.mk b/libsensors/software/core/mllite/build/android/static.mk deleted file mode 100644 index 6ad45de..0000000 --- a/libsensors/software/core/mllite/build/android/static.mk +++ /dev/null @@ -1,110 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -OBJFOLDER = $(CURDIR)/obj - -CROSS = arm-none-linux-gnueabi- -COMP= $(CROSS)gcc -LINK= $(CROSS)ar cr - -MLLITE_DIR = $(MLSDK_ROOT)/mllite -MPL_DIR = $(MLSDK_ROOT)/mldmp -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform - -include $(MLSDK_ROOT)/Android-common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += -Wall -fpic -CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -DLINUX -DANDROID -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(MLPLATFORM_DIR)/include -CFLAGS += -I$(MLSDK_ROOT)/mlutils -CFLAGS += -I$(MLSDK_ROOT)/mlapps/common -CFLAGS += $(MLSDK_INCLUDES) -CFLAGS += $(MLSDK_DEFINES) - -VPATH += $(MLLITE_DIR) -VPATH += $(MLSDK_ROOT)/mlutils -VPATH += $(MLLITE_DIR)/accel -VPATH += $(MLLITE_DIR)/compass -VPATH += $(MLLITE_DIR)/pressure -VPATH += $(MLLITE_DIR)/mlapps/common - -#################################################################################################### -## sources - -ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) - -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c -ML_SOURCES += $(MLLITE_DIR)/accel.c -ML_SOURCES += $(MLLITE_DIR)/compass.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c -ML_SOURCES += $(MLLITE_DIR)/key0_96.c -ML_SOURCES += $(MLLITE_DIR)/pressure.c -ML_SOURCES += $(MLLITE_DIR)/ml.c -ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c -ML_SOURCES += $(MLLITE_DIR)/ml_init.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c -ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c -ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c -ML_SOURCES += $(MLLITE_DIR)/mldl.c -ML_SOURCES += $(MLLITE_DIR)/mldmp.c -ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c -ML_SOURCES += $(MLLITE_DIR)/mlstates.c -ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c -ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c -ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c -ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c -ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c -ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c -ifeq ($(MPU_NAME),MPU3050) -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c -else -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c -endif - -ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) -ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all clean cleanall - -all: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(ML_OBJS_DST)\n") - $(LINK) $(LIBRARY) $(ML_OBJS_DST) - $(CROSS)ranlib $(LIBRARY) - -$(OBJFOLDER) : - @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") - mkdir obj - -$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") - $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/libsensors/software/core/mllite/build/filelist.mk b/libsensors/software/core/mllite/build/filelist.mk deleted file mode 100644 index 011120c..0000000 --- a/libsensors/software/core/mllite/build/filelist.mk +++ /dev/null @@ -1,42 +0,0 @@ -#### filelist.mk for mllite #### - -# headers only -HEADERS := $(MLLITE_DIR)/invensense.h - -# headers for sources -HEADERS += $(MLLITE_DIR)/data_builder.h -HEADERS += $(MLLITE_DIR)/hal_outputs.h -HEADERS += $(MLLITE_DIR)/message_layer.h -HEADERS += $(MLLITE_DIR)/ml_math_func.h -HEADERS += $(MLLITE_DIR)/mpl.h -HEADERS += $(MLLITE_DIR)/results_holder.h -HEADERS += $(MLLITE_DIR)/start_manager.h -HEADERS += $(MLLITE_DIR)/storage_manager.h - -# headers (linux specific) -HEADERS += $(MLLITE_DIR)/linux/mlos.h -HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h -HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h -HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h - -# sources -SOURCES := $(MLLITE_DIR)/data_builder.c -SOURCES += $(MLLITE_DIR)/hal_outputs.c -SOURCES += $(MLLITE_DIR)/message_layer.c -SOURCES += $(MLLITE_DIR)/ml_math_func.c -SOURCES += $(MLLITE_DIR)/mpl.c -SOURCES += $(MLLITE_DIR)/results_holder.c -SOURCES += $(MLLITE_DIR)/start_manager.c -SOURCES += $(MLLITE_DIR)/storage_manager.c - -# sources (linux specific) -SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c -SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c -SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c -SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c - - -INV_SOURCES += $(SOURCES) - -VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux - diff --git a/libsensors/software/core/mllite/data_builder.c b/libsensors/software/core/mllite/data_builder.c deleted file mode 100644 index f70be7c..0000000 --- a/libsensors/software/core/mllite/data_builder.c +++ /dev/null @@ -1,1162 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/** - * @defgroup Data_Builder data_builder - * @brief Motion Library - Data Builder - * Constructs and Creates the data for MPL - * - * @{ - * @file data_builder.c - * @brief Data Builder. - */ - -#undef MPL_LOG_NDEBUG -#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ - -#include "ml_math_func.h" -#include "data_builder.h" -#include "mlmath.h" -#include "storage_manager.h" -#include "message_layer.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL" - -typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); - -struct process_t { - inv_process_cb_func func; - int priority; - int data_required; -}; - -struct inv_db_save_t { - /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ - long compass_bias[3]; - /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ - long gyro_bias[3]; - /** Temperature when *gyro_bias was stored. */ - long gyro_temp; - /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ - long accel_bias[3]; - /** Temperature when accel bias was stored. */ - long accel_temp; - long gyro_temp_slope[3]; -}; - -struct inv_data_builder_t { - int num_cb; - struct process_t process[INV_MAX_DATA_CB]; - struct inv_db_save_t save; - int compass_disturbance; -#ifdef INV_PLAYBACK_DBG - int debug_mode; - int last_mode; - FILE *file; -#endif -}; - -void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); -static void inv_set_contiguous(void); - -static struct inv_data_builder_t inv_data_builder; -static struct inv_sensor_cal_t sensors; - -/** Change this key if the data being stored by this file changes */ -#define INV_DB_SAVE_KEY 53394 - -#ifdef INV_PLAYBACK_DBG - -/** Turn on data logging to allow playback of same scenario at a later time. -* @param[in] file File to write to, must be open. -*/ -void inv_turn_on_data_logging(FILE *file) -{ - MPL_LOGV("input data logging started\n"); - inv_data_builder.file = file; - inv_data_builder.debug_mode = RD_RECORD; -} - -/** Turn off data logging to allow playback of same scenario at a later time. -* File passed to inv_turn_on_data_logging() must be closed after calling this. -*/ -void inv_turn_off_data_logging() -{ - MPL_LOGV("input data logging stopped\n"); - inv_data_builder.debug_mode = RD_NO_DEBUG; - inv_data_builder.file = NULL; -} -#endif - -/** This function receives the data that was stored in non-volatile memory between power off */ -static inv_error_t inv_db_load_func(const unsigned char *data) -{ - memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); - return INV_SUCCESS; -} - -/** This function returns the data to be stored in non-volatile memory between power off */ -static inv_error_t inv_db_save_func(unsigned char *data) -{ - memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); - return INV_SUCCESS; -} - -/** Initialize the data builder -*/ -inv_error_t inv_init_data_builder(void) -{ - /* TODO: Hardcode temperature scale/offset here. */ - memset(&inv_data_builder, 0, sizeof(inv_data_builder)); - memset(&sensors, 0, sizeof(sensors)); - return inv_register_load_store(inv_db_load_func, inv_db_save_func, - sizeof(inv_data_builder.save), - INV_DB_SAVE_KEY); -} - -/** Gyro sensitivity. -* @return A scale factor to convert device units to degrees per second scaled by 2^16 -* such that degrees_per_second = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum rate * 2^15. -*/ -long inv_get_gyro_sensitivity() -{ - return sensors.gyro.sensitivity; -} - -/** Accel sensitivity. -* @return A scale factor to convert device units to g's scaled by 2^16 -* such that g_s = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum accel value in g's * 2^15. -*/ -long inv_get_accel_sensitivity(void) -{ - return sensors.accel.sensitivity; -} - -/** Compass sensitivity. -* @return A scale factor to convert device units to micro Tesla scaled by 2^16 -* such that uT = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum uT * 2^15. -*/ -long inv_get_compass_sensitivity(void) -{ - return sensors.compass.sensitivity; -} - -/** Sets orientation and sensitivity field for a sensor. -* @param[out] sensor Structure to apply settings to -* @param[in] orientation Orientation description of how part is mounted. -* @param[in] sensitivity A Scale factor to convert from hardware units to -* standard units (dps, uT, g). -*/ -void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, - int orientation, long sensitivity) -{ - sensor->sensitivity = sensitivity; - sensor->orientation = orientation; -} -void inv_get_quaternion_transformation(int orientation, long *q) -{ - long s = 759250125; // sqrt(.5)*2^30 - - switch (orientation) - { - case 0x70: - q[0] = 759250125; - q[1] = 759250125; - q[2] = 0; - q[3] = 0; - break; - case 0x10a: - q[0] = 759250125; - q[1] = 0; - q[2] = 759250125; - q[3] = 0; - break; - case 0x85: - q[0] = 759250125; - q[1] = 0; - q[2] = 0; - q[3] = 759250125; - break; - case 0x181: - q[0] = 0; - q[1] = 759250125; - q[2] = 759250125; - q[3] = 0; - break; - case 0x2a: - q[0] = 0; - q[1] = 759250125; - q[2] = 0; - q[3] = 759250125; - break; - case 0x54: - q[0] = 0; - q[1] = 0; - q[2] = 759250125; - q[3] = 759250125; - break; - case 0x150: - q[0] = 759250125; - q[1] = -759250125; - q[2] = 0; - q[3] = 0; - break; - case 0xe: - q[0] = 759250125; - q[1] = 0; - q[2] = -759250125; - q[3] = 0; - break; - case 0xa1: - q[0] = 759250125; - q[1] = 0; - q[2] = 0; - q[3] = -759250125; - break; - case 0x1a5: - q[0] = 0; - q[1] = 759250125; - q[2] = -759250125; - q[3] = 0; - break; - case 0x12e: - q[0] = 0; - q[1] = 759250125; - q[2] = 0; - q[3] = -759250125; - break; - case 0x174: - q[0] = 0; - q[1] = 0; - q[2] = 759250125; - q[3] = -759250125; - break; - - - case 0x88: - q[0] = 1073741824; - q[1] = 0; - q[2] = 0; - q[3] = 0; - break; - - - case 0x1a8: - q[0] = 0; - q[1] = 1073741824; - q[2] = 0; - q[3] = 0; - break; - - case 0x18c: - q[0] = 0; - q[1] = 0; - q[2] = 1073741824; - q[3] = 0; - break; - - case 0xac: - q[0] = 0; - q[1] = 0; - q[2] = 0; - q[3] = 1073741824; - break; - - default: - break; - } -} -/** Sets the Orientation and Sensitivity of the gyro data. -* @param[in] orientation A scalar defining the transformation from chip mounting -* to the body frame. The function inv_orientation_matrix_to_scalar() -* can convert the transformation matrix to this scalar and describes the -* scalar in further detail. -* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 -* such that degrees_per_second = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum rate * 2^15. -*/ -void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_G_ORIENT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); - fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); - } -#endif - set_sensor_orientation_and_scale(&sensors.gyro, orientation, - sensitivity); -} - -/** Set Gyro Sample rate in micro seconds. -* @param[in] sample_rate_us Set Gyro Sample rate in us -*/ -void inv_set_gyro_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.gyro.sample_rate_us = sample_rate_us; - sensors.gyro.sample_rate_ms = sample_rate_us / 1000; - if (sensors.gyro.bandwidth == 0) { - sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); - } -} - -/** Set Accel Sample rate in micro seconds. -* @param[in] sample_rate_us Set Accel Sample rate in us -*/ -void inv_set_accel_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.accel.sample_rate_us = sample_rate_us; - sensors.accel.sample_rate_ms = sample_rate_us / 1000; - if (sensors.accel.bandwidth == 0) { - sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); - } -} - -/** Set Compass Sample rate in micro seconds. -* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. -*/ -void inv_set_compass_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.compass.sample_rate_us = sample_rate_us; - sensors.compass.sample_rate_ms = sample_rate_us / 1000; - if (sensors.compass.bandwidth == 0) { - sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); - } -} -/** Set Quat Sample rate in micro seconds. -* @param[in] sample_rate_us Set Quat Sample rate in us -*/ -void inv_set_quat_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.quat.sample_rate_us = sample_rate_us; - sensors.quat.sample_rate_ms = sample_rate_us / 1000; -} - -/** Set Gyro Bandwidth in Hz -* @param[in] bandwidth_hz Gyro bandwidth in Hz -*/ -void inv_set_gyro_bandwidth(int bandwidth_hz) -{ - sensors.gyro.bandwidth = bandwidth_hz; -} - -/** Set Accel Bandwidth in Hz -* @param[in] bandwidth_hz Gyro bandwidth in Hz -*/ -void inv_set_accel_bandwidth(int bandwidth_hz) -{ - sensors.accel.bandwidth = bandwidth_hz; -} - -/** Set Compass Bandwidth in Hz -* @param[in] bandwidth_hz Gyro bandwidth in Hz -*/ -void inv_set_compass_bandwidth(int bandwidth_hz) -{ - sensors.compass.bandwidth = bandwidth_hz; -} - -/** Helper function stating whether the compass is on or off. - * @return TRUE if compass if on, 0 if compass if off -*/ -int inv_get_compass_on() -{ - return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; -} - -/** Helper function stating whether the gyro is on or off. - * @return TRUE if gyro if on, 0 if gyro if off -*/ -int inv_get_gyro_on() -{ - return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; -} - -/** Helper function stating whether the acceleromter is on or off. - * @return TRUE if accel if on, 0 if accel if off -*/ -int inv_get_accel_on() -{ - return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; -} - -/** Get last timestamp across all 3 sensors that are on. -* This find out which timestamp has the largest value for sensors that are on. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_time_t inv_get_last_timestamp() -{ - inv_time_t timestamp = 0; - if (sensors.accel.status & INV_SENSOR_ON) { - timestamp = sensors.accel.timestamp; - } - if (sensors.gyro.status & INV_SENSOR_ON) { - if (timestamp < sensors.gyro.timestamp) { - timestamp = sensors.gyro.timestamp; - } - } - if (sensors.compass.status & INV_SENSOR_ON) { - if (timestamp < sensors.compass.timestamp) { - timestamp = sensors.compass.timestamp; - } - } - if (sensors.temp.status & INV_SENSOR_ON) { - if (timestamp < sensors.temp.timestamp) - timestamp = sensors.temp.timestamp; - } - return timestamp; -} - -/** Sets the orientation and sensitivity of the gyro data. -* @param[in] orientation A scalar defining the transformation from chip mounting -* to the body frame. The function inv_orientation_matrix_to_scalar() -* can convert the transformation matrix to this scalar and describes the -* scalar in further detail. -* @param[in] sensitivity A scale factor to convert device units to g's -* such that g's = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum g_value * 2^15. -*/ -void inv_set_accel_orientation_and_scale(int orientation, long sensitivity) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_A_ORIENT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); - fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); - } -#endif - set_sensor_orientation_and_scale(&sensors.accel, orientation, - sensitivity); -} - -/** Sets the Orientation and Sensitivity of the gyro data. -* @param[in] orientation A scalar defining the transformation from chip mounting -* to the body frame. The function inv_orientation_matrix_to_scalar() -* can convert the transformation matrix to this scalar and describes the -* scalar in further detail. -* @param[in] sensitivity A scale factor to convert device units to uT -* such that uT = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum uT_value * 2^15. -*/ -void inv_set_compass_orientation_and_scale(int orientation, long sensitivity) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_C_ORIENT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); - fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); - } -#endif - set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); -} - -void inv_matrix_vector_mult(const long *A, const long *x, long *y) -{ - y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); - y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); - y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); -} - -/** Takes raw data stored in the sensor, removes bias, and converts it to -* calibrated data in the body frame. -* @param[in,out] sensor structure to modify -* @param[in] bias bias in the mounting frame, in hardware units scaled by -* 2^16. Length 3. -*/ -void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) -{ - long raw32[3]; - - // Convert raw to calibrated - raw32[0] = (long)sensor->raw[0] << 16; - raw32[1] = (long)sensor->raw[1] << 16; - raw32[2] = (long)sensor->raw[2] << 16; - - raw32[0] -= bias[0]; - raw32[1] -= bias[1]; - raw32[2] -= bias[2]; - - inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated); - - sensor->status |= INV_CALIBRATED; -} - -/** Returns the current bias for the compass -* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. -* Length 3. -*/ -void inv_get_compass_bias(long *bias) -{ - if (bias != NULL) { - memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); - } -} - -void inv_set_compass_bias(const long *bias, int accuracy) -{ - if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { - memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); - inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); - } - sensors.compass.accuracy = accuracy; - inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); -} - -/** Set the state of a compass disturbance -* @param[in] dist 1=disturbance, 0=no disturbance -*/ -void inv_set_compass_disturbance(int dist) -{ - inv_data_builder.compass_disturbance = dist; -} - -int inv_get_compass_disturbance(void) { - return inv_data_builder.compass_disturbance; -} -/** Sets the accel bias. -* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame -* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. -*/ -void inv_set_accel_bias(const long *bias, int accuracy) -{ - if (bias) { - if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) { - memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias)); - inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); - } - } - sensors.accel.accuracy = accuracy; - inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); -} - -/** Sets the accel bias with control over which axis. -* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame -* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. -* @param[in] mask Mask to select axis to apply bias set. -*/ -void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) -{ - if (bias) { - if (mask & 1){ - inv_data_builder.save.accel_bias[0] = bias[0]; - } - if (mask & 2){ - inv_data_builder.save.accel_bias[1] = bias[1]; - } - if (mask & 4){ - inv_data_builder.save.accel_bias[2] = bias[2]; - } - - inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); - } - sensors.accel.accuracy = accuracy; -} - - -/** Sets the gyro bias -* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. -* Length 3. -* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. -*/ -void inv_set_gyro_bias(const long *bias, int accuracy) -{ - if (bias != NULL) { - if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { - memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); - inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); - } - } - sensors.gyro.accuracy = accuracy; - /* TODO: What should we do if there's no temperature data? */ - if (sensors.temp.calibrated[0]) - inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; - else - /* Set to 27 deg C for now until we've got a better solution. */ - inv_data_builder.save.gyro_temp = 1769472L; - inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); -} - -/* TODO: Add this information to inv_sensor_cal_t */ -/** - * Get the gyro biases and temperature record from MPL - * @param[in] bias - * Gyro bias in hardware units scaled by 2^16. - * In chip mounting frame. - * Length 3. - * @param[in] temp - * Tempearature in degrees C. - */ -void inv_get_gyro_bias(long *bias, long *temp) -{ - if (bias != NULL) - memcpy(bias, inv_data_builder.save.gyro_bias, - sizeof(inv_data_builder.save.gyro_bias)); - if (temp != NULL) - temp[0] = inv_data_builder.save.gyro_temp; -} - -/** Get Accel Bias -* @param[out] bias Accel bias where -* @param[out] temp Temperature where 1 C = 2^16 -*/ -void inv_get_accel_bias(long *bias, long *temp) -{ - if (bias != NULL) - memcpy(bias, inv_data_builder.save.accel_bias, - sizeof(inv_data_builder.save.accel_bias)); - if (temp != NULL) - temp[0] = inv_data_builder.save.accel_temp; -} - -/** - * Record new accel data for use when inv_execute_on_data() is called - * @param[in] accel accel data. - * Length 3. - * Calibrated data is in m/s^2 scaled by 2^16 in body frame. - * Raw data is in device units in chip mounting frame. - * @param[in] status - * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 - * being most accurate. - * The upper bit INV_CALIBRATED, is set if the data was calibrated - * outside MPL and it is not set if the data being passed is raw. - * Raw data should be in device units, typically in a 16-bit range. - * @param[in] timestamp - * Monotonic time stamp, for Android it's in nanoseconds. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_ACCEL; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - if ((status & INV_CALIBRATED) == 0) { - sensors.accel.raw[0] = (short)accel[0]; - sensors.accel.raw[1] = (short)accel[1]; - sensors.accel.raw[2] = (short)accel[2]; - sensors.accel.status |= INV_RAW_DATA; - inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); - } else { - sensors.accel.calibrated[0] = accel[0]; - sensors.accel.calibrated[1] = accel[1]; - sensors.accel.calibrated[2] = accel[2]; - sensors.accel.status |= INV_CALIBRATED; - sensors.accel.accuracy = status & 3; - } - sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; - sensors.accel.timestamp_prev = sensors.accel.timestamp; - sensors.accel.timestamp = timestamp; - - return INV_SUCCESS; -} - -/** Record new gyro data and calls inv_execute_on_data() if previous -* sample has not been processed. -* @param[in] gyro Data is in device units. Length 3. -* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. -* @param[out] executed Set to 1 if data processing was done. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_GYRO; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); - sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; - sensors.gyro.timestamp_prev = sensors.gyro.timestamp; - sensors.gyro.timestamp = timestamp; - inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); - - return INV_SUCCESS; -} - -/** Record new compass data for use when inv_execute_on_data() is called -* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. -* Length 3. -* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. -* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is -* not set if the data being passed is raw. Raw data should be in device units, typically -* in a 16-bit range. -* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. -* @param[out] executed Set to 1 if data processing was done. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_build_compass(const long *compass, int status, - inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_COMPASS; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - if ((status & INV_CALIBRATED) == 0) { - sensors.compass.raw[0] = (short)compass[0]; - sensors.compass.raw[1] = (short)compass[1]; - sensors.compass.raw[2] = (short)compass[2]; - inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); - sensors.compass.status |= INV_RAW_DATA; - } else { - sensors.compass.calibrated[0] = compass[0]; - sensors.compass.calibrated[1] = compass[1]; - sensors.compass.calibrated[2] = compass[2]; - sensors.compass.status |= INV_CALIBRATED; - sensors.compass.accuracy = status & 3; - } - sensors.compass.timestamp_prev = sensors.compass.timestamp; - sensors.compass.timestamp = timestamp; - sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; - - return INV_SUCCESS; -} - -/** Record new temperature data for use when inv_execute_on_data() is called. - * @param[in] temp Temperature data in q16 format. - * @param[in] timestamp Monotonic time stamp; for Android it's in - * nanoseconds. -* @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_TEMPERATURE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - sensors.temp.calibrated[0] = temp; - sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; - sensors.temp.timestamp_prev = sensors.temp.timestamp; - sensors.temp.timestamp = timestamp; - /* TODO: Apply scale, remove offset. */ - - return INV_SUCCESS; -} -/** quaternion data -* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. -* Real part first. Length 4. -* @param[in] status number of axis, 16-bit or 32-bit -* @param[in] timestamp -* @param[in] timestamp Monotonic time stamp; for Android it's in -* nanoseconds. -* @param[out] executed Set to 1 if data processing was done. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_QUAT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); - sensors.quat.timestamp = timestamp; - sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; - sensors.quat.status |= (INV_BIAS_APPLIED & status); - - return INV_SUCCESS; -} - -/** This should be called when the accel has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_accel_was_turned_off() -{ - sensors.accel.status = 0; -} - -/** This should be called when the compass has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_compass_was_turned_off() -{ - sensors.compass.status = 0; -} - -/** This should be called when the quaternion data from the DMP has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_quaternion_sensor_was_turned_off(void) -{ - sensors.quat.status = 0; -} - -/** This should be called when the gyro has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_gyro_was_turned_off() -{ - sensors.gyro.status = 0; -} - -/** This should be called when the temperature sensor has been turned off. - * This is so that we will know if the data is contiguous. - */ -void inv_temperature_was_turned_off() -{ - sensors.temp.status = 0; -} - -/** Registers to receive a callback when there is new sensor data. -* @internal -* @param[in] func Function pointer to receive callback when there is new sensor data -* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority -* numbers must be unique. -* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be -* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = -* gyro data, INV_MAG_NEW = compass data. So passing in -* INV_ACCEL_NEW | INV_MAG_NEW, a -* callback would be generated if there was new magnetomer data OR new accel data. -*/ -inv_error_t inv_register_data_cb( - inv_error_t (*func)(struct inv_sensor_cal_t *data), - int priority, int sensor_type) -{ - inv_error_t result = INV_SUCCESS; - int kk, nn; - - // Make sure we haven't registered this function already - // Or used the same priority - for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if ((inv_data_builder.process[kk].func == func) || - (inv_data_builder.process[kk].priority == priority)) { - return INV_ERROR_INVALID_PARAMETER; //fixme give a warning - } - } - - // Make sure we have not filled up our number of allowable callbacks - if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { - kk = 0; - if (inv_data_builder.num_cb != 0) { - // set kk to be where this new callback goes in the array - while ((kk < inv_data_builder.num_cb) && - (inv_data_builder.process[kk].priority < priority)) { - kk++; - } - if (kk != inv_data_builder.num_cb) { - // We need to move the others - for (nn = inv_data_builder.num_cb; nn > kk; --nn) { - inv_data_builder.process[nn] = - inv_data_builder.process[nn - 1]; - } - } - } - // Add new callback - inv_data_builder.process[kk].func = func; - inv_data_builder.process[kk].priority = priority; - inv_data_builder.process[kk].data_required = sensor_type; - inv_data_builder.num_cb++; - } else { - MPL_LOGE("Unable to add feature callback as too many were already registered\n"); - result = INV_ERROR_MEMORY_EXAUSTED; - } - - return result; -} - -/** Unregisters the callback that happens when new sensor data is received. -* @internal -* @param[in] func Function pointer to receive callback when there is new sensor data -* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority -* numbers must be unique. -* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be -* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = -* gyro data, INV_MAG_NEW = compass data. So passing in -* INV_ACCEL_NEW | INV_MAG_NEW, a -* callback would be generated if there was new magnetomer data OR new accel data. -*/ -inv_error_t inv_unregister_data_cb( - inv_error_t (*func)(struct inv_sensor_cal_t *data)) -{ - int kk, nn; - - for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if (inv_data_builder.process[kk].func == func) { - // Delete this callback - for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { - inv_data_builder.process[nn - 1] = - inv_data_builder.process[nn]; - } - inv_data_builder.num_cb--; - return INV_SUCCESS; - } - } - - return INV_SUCCESS; // We did not find the callback -} - -/** After at least one of inv_build_gyro(), inv_build_accel(), or -* inv_build_compass() has been called, this function should be called. -* It will process the data it has received and update all the internal states -* and features that have been turned on. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_execute_on_data(void) -{ - inv_error_t result, first_error; - int kk; - int mode; - -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_EXECUTE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - } -#endif - // Determine what new data we have - mode = 0; - if (sensors.gyro.status & INV_NEW_DATA) - mode |= INV_GYRO_NEW; - if (sensors.accel.status & INV_NEW_DATA) - mode |= INV_ACCEL_NEW; - if (sensors.compass.status & INV_NEW_DATA) - mode |= INV_MAG_NEW; - if (sensors.temp.status & INV_NEW_DATA) - mode |= INV_TEMP_NEW; - if (sensors.quat.status & INV_QUAT_NEW) - mode |= INV_QUAT_NEW; - - first_error = INV_SUCCESS; - - for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if (mode & inv_data_builder.process[kk].data_required) { - result = inv_data_builder.process[kk].func(&sensors); - if (result && !first_error) { - first_error = result; - } - } - } - - inv_set_contiguous(); - - return first_error; -} - -/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. -* -*/ -static void inv_set_contiguous(void) -{ - inv_time_t current_time = 0; - if (sensors.gyro.status & INV_NEW_DATA) { - sensors.gyro.status |= INV_CONTIGUOUS; - current_time = sensors.gyro.timestamp; - } - if (sensors.accel.status & INV_NEW_DATA) { - sensors.accel.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.accel.timestamp); - } - if (sensors.compass.status & INV_NEW_DATA) { - sensors.compass.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.compass.timestamp); - } - if (sensors.temp.status & INV_NEW_DATA) { - sensors.temp.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.temp.timestamp); - } - if (sensors.quat.status & INV_NEW_DATA) { - sensors.quat.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.quat.timestamp); - } - -#if 0 - /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() - * type functions. This is just in case that breaks down. We make sure - * all the data is within 2 seconds of the newest piece of data*/ - if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) - inv_gyro_was_turned_off(); - if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) - inv_accel_was_turned_off(); - if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) - inv_compass_was_turned_off(); - /* TODO: Temperature might not need to be read this quickly. */ - if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) - inv_temperature_was_turned_off(); -#endif - - /* clear bits */ - sensors.gyro.status &= ~INV_NEW_DATA; - sensors.accel.status &= ~INV_NEW_DATA; - sensors.compass.status &= ~INV_NEW_DATA; - sensors.temp.status &= ~INV_NEW_DATA; - sensors.quat.status &= ~INV_NEW_DATA; -} - -/** Gets a whole set of accel data including data, accuracy and timestamp. - * @param[out] data Accel Data where 1g = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - if (data != NULL) { - memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); - } - if (timestamp != NULL) { - *timestamp = sensors.accel.timestamp; - } - if (accuracy != NULL) { - *accuracy = sensors.accel.accuracy; - } -} - -/** Gets a whole set of gyro data including data, accuracy and timestamp. - * @param[out] data Gyro Data where 1 dps = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); - if (timestamp != NULL) { - *timestamp = sensors.gyro.timestamp; - } - if (accuracy != NULL) { - *accuracy = sensors.gyro.accuracy; - } -} - -/** Get's latest gyro data. -* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. -*/ -void inv_get_gyro(long *gyro) -{ - memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); -} - -/** Gets a whole set of compass data including data, accuracy and timestamp. - * @param[out] data Compass Data where 1 uT = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); - if (timestamp != NULL) { - *timestamp = sensors.compass.timestamp; - } - if (accuracy != NULL) { - if (inv_data_builder.compass_disturbance) - *accuracy = 0; - else - *accuracy = sensors.compass.accuracy; - } -} - -/** Gets a whole set of temperature data including data, accuracy and timestamp. - * @param[out] data Temperature data where 1 degree C = 2^16 - * @param[out] accuracy 0 to 3, where 3 is most accurate. - * @param[out] timestamp The timestamp of the data sample. - */ -void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) -{ - data[0] = sensors.temp.calibrated[0]; - if (timestamp) - *timestamp = sensors.temp.timestamp; - if (accuracy) - *accuracy = sensors.temp.accuracy; -} - -/** Returns accuracy of gyro. - * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. -*/ -int inv_get_gyro_accuracy(void) -{ - return sensors.gyro.accuracy; -} - -/** Returns accuracy of compass. - * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. -*/ -int inv_get_mag_accuracy(void) -{ - if (inv_data_builder.compass_disturbance) - return 0; - return sensors.compass.accuracy; -} - -/** Returns accuracy of accel. - * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. -*/ -int inv_get_accel_accuracy(void) -{ - return sensors.accel.accuracy; -} - -inv_error_t inv_get_gyro_orient(int *orient) -{ - *orient = sensors.gyro.orientation; - return 0; -} - -inv_error_t inv_get_accel_orient(int *orient) -{ - *orient = sensors.accel.orientation; - return 0; -} - - -/** - * @} - */ diff --git a/libsensors/software/core/mllite/data_builder.h b/libsensors/software/core/mllite/data_builder.h deleted file mode 100644 index b2d0881..0000000 --- a/libsensors/software/core/mllite/data_builder.h +++ /dev/null @@ -1,224 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_DATA_BUILDER_H__ -#define INV_DATA_BUILDER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -// Uncomment this flag to enable playback debug and record or playback scenarios -//#define INV_PLAYBACK_DBG - -/** This is a new sample of accel data */ -#define INV_ACCEL_NEW 1 -/** This is a new sample of gyro data */ -#define INV_GYRO_NEW 2 -/** This is a new sample of compass data */ -#define INV_MAG_NEW 4 -/** This is a new sample of temperature data */ -#define INV_TEMP_NEW 8 -/** This is a new sample of quaternion data */ -#define INV_QUAT_NEW 16 - -/** Set if the data is contiguous. Typically not set if a sample was skipped */ -#define INV_CONTIGUOUS 16 -/** Set if the calibrated data has been solved for */ -#define INV_CALIBRATED 32 -/* INV_NEW_DATA set for a new set of data, cleared if not available. */ -#define INV_NEW_DATA 64 -/* Set if raw data exists */ -#define INV_RAW_DATA 128 -/* Set if the sensor is on */ -#define INV_SENSOR_ON 256 -/* Set if quaternion has bias correction applied */ -#define INV_BIAS_APPLIED 512 - -#define INV_PRIORITY_MOTION_NO_MOTION 100 -#define INV_PRIORITY_GYRO_TC 150 -#define INV_PRIORITY_QUATERNION_GYRO_ACCEL 200 -#define INV_PRIORITY_QUATERNION_NO_GYRO 250 -#define INV_PRIORITY_MAGNETIC_DISTURBANCE 300 -#define INV_PRIORITY_HEADING_FROM_GYRO 350 -#define INV_PRIORITY_COMPASS_BIAS_W_GYRO 375 -#define INV_PRIORITY_COMPASS_VECTOR_CAL 400 -#define INV_PRIORITY_COMPASS_ADV_BIAS 500 -#define INV_PRIORITY_9_AXIS_FUSION 600 -#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS 700 -#define INV_PRIORITY_QUATERNION_ACCURACY 750 -#define INV_PRIORITY_RESULTS_HOLDER 800 -#define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850 -#define INV_PRIORITY_HAL_OUTPUTS 900 -#define INV_PRIORITY_GLYPH 950 -#define INV_PRIORITY_SM 1000 - -struct inv_single_sensor_t { - /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when - * the rotation matrix could be thought of only having elements of 0,1,-1. - * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign. - * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row. - * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row. - * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row. - */ - int orientation; - /** The raw data in raw data units in the mounting frame */ - short raw[3]; - /** Calibrated data */ - long calibrated[3]; - long sensitivity; - /** Sample rate in microseconds */ - long sample_rate_us; - long sample_rate_ms; - /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample - * skipped due to power savings turning off this sensor. - * INV_NEW_DATA set for a new set of data, cleared if not available. - * INV_CALIBRATED_SET if calibrated data has been solved for */ - int status; - /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */ - int accuracy; - inv_time_t timestamp; - inv_time_t timestamp_prev; - /** Bandwidth in Hz */ - int bandwidth; -}; -struct inv_quat_sensor_t { - long raw[4]; - /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample - * skipped due to power savings turning off this sensor. - * INV_NEW_DATA set for a new set of data, cleared if not available. - * INV_CALIBRATED_SET if calibrated data has been solved for */ - int status; - inv_time_t timestamp; - long sample_rate_us; - long sample_rate_ms; -}; - -struct inv_sensor_cal_t { - struct inv_single_sensor_t gyro; - struct inv_single_sensor_t accel; - struct inv_single_sensor_t compass; - struct inv_single_sensor_t temp; - struct inv_quat_sensor_t quat; - /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate - * which data is a new sample as these data points may have different sample rates. - */ - int status; -}; - -// Useful for debug record and playback -typedef enum { - RD_NO_DEBUG, - RD_RECORD, - RD_PLAYBACK -} rd_dbg_mode; - -typedef enum { - PLAYBACK_DBG_TYPE_GYRO, - PLAYBACK_DBG_TYPE_ACCEL, - PLAYBACK_DBG_TYPE_COMPASS, - PLAYBACK_DBG_TYPE_TEMPERATURE, - PLAYBACK_DBG_TYPE_EXECUTE, - PLAYBACK_DBG_TYPE_A_ORIENT, - PLAYBACK_DBG_TYPE_G_ORIENT, - PLAYBACK_DBG_TYPE_C_ORIENT, - PLAYBACK_DBG_TYPE_A_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_C_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_G_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_GYRO_OFF, - PLAYBACK_DBG_TYPE_ACCEL_OFF, - PLAYBACK_DBG_TYPE_COMPASS_OFF, - PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_QUAT - -} inv_rd_dbg_states; - -/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/ -#define INV_MAX_DATA_CB 20 - -#ifdef INV_PLAYBACK_DBG -#include <stdio.h> -void inv_turn_on_data_logging(FILE *file); -void inv_turn_off_data_logging(); -#endif - -void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity); -void inv_set_accel_orientation_and_scale(int orientation, - long sensitivity); -void inv_set_compass_orientation_and_scale(int orientation, - long sensitivity); -void inv_set_gyro_sample_rate(long sample_rate_us); -void inv_set_compass_sample_rate(long sample_rate_us); -void inv_set_quat_sample_rate(long sample_rate_us); -void inv_set_accel_sample_rate(long sample_rate_us); -void inv_set_gyro_bandwidth(int bandwidth_hz); -void inv_set_accel_bandwidth(int bandwidth_hz); -void inv_set_compass_bandwidth(int bandwidth_hz); - -inv_error_t inv_register_data_cb(inv_error_t (*func) - (struct inv_sensor_cal_t * data), int priority, - int sensor_type); -inv_error_t inv_unregister_data_cb(inv_error_t (*func) - (struct inv_sensor_cal_t * data)); - -inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp); -inv_error_t inv_build_compass(const long *compass, int status, - inv_time_t timestamp); -inv_error_t inv_build_accel(const long *accel, int status, - inv_time_t timestamp); -inv_error_t inv_build_temp(const long temp, inv_time_t timestamp); -inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp); -inv_error_t inv_execute_on_data(void); - -void inv_get_compass_bias(long *bias); - -void inv_set_compass_bias(const long *bias, int accuracy); -void inv_set_compass_disturbance(int dist); -void inv_set_gyro_bias(const long *bias, int accuracy); -void inv_set_accel_bias(const long *bias, int accuracy); -void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); - -void inv_get_gyro_bias(long *bias, long *temp); -void inv_get_accel_bias(long *bias, long *temp); - -void inv_gyro_was_turned_off(void); -void inv_accel_was_turned_off(void); -void inv_compass_was_turned_off(void); -void inv_quaternion_sensor_was_turned_off(void); -inv_error_t inv_init_data_builder(void); -long inv_get_gyro_sensitivity(void); -long inv_get_accel_sensitivity(void); -long inv_get_compass_sensitivity(void); - -void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp); -void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp); -void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp); - -void inv_get_gyro(long *gyro); - -int inv_get_gyro_accuracy(void); -int inv_get_accel_accuracy(void); -int inv_get_mag_accuracy(void); - -int inv_get_compass_on(void); -int inv_get_gyro_on(void); -int inv_get_accel_on(void); - -inv_time_t inv_get_last_timestamp(void); -int inv_get_compass_disturbance(void); - -//New DMP Cal Functions -inv_error_t inv_get_gyro_orient(int *orient); -inv_error_t inv_get_accel_orient(int *orient); - - -#ifdef __cplusplus -} -#endif - -#endif /* INV_DATA_BUILDER_H__ */ diff --git a/libsensors/software/core/mllite/dmpconfig.txt b/libsensors/software/core/mllite/dmpconfig.txt deleted file mode 100644 index 4643ed5..0000000 --- a/libsensors/software/core/mllite/dmpconfig.txt +++ /dev/null @@ -1,3 +0,0 @@ -major version = 1 -minor version = 0 -startAddr = 0 diff --git a/libsensors/software/core/mllite/hal_outputs.c b/libsensors/software/core/mllite/hal_outputs.c deleted file mode 100644 index 1cd3b81..0000000 --- a/libsensors/software/core/mllite/hal_outputs.c +++ /dev/null @@ -1,425 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/** - * @defgroup HAL_Outputs hal_outputs - * @brief Motion Library - HAL Outputs - * Sets up common outputs for HAL - * - * @{ - * @file hal_outputs.c - * @brief HAL Outputs. - */ -#include "hal_outputs.h" -#include "log.h" -#include "ml_math_func.h" -#include "mlmath.h" -#include "start_manager.h" -#include "data_builder.h" -#include "results_holder.h" - -struct hal_output_t { - int accuracy_mag; /**< Compass accuracy */ - int accuracy_gyro; /**< Gyro Accuracy */ - int accuracy_accel; /**< Accel Accuracy */ - inv_time_t nav_timestamp; - inv_time_t gam_timestamp; - inv_time_t accel_timestamp; - long nav_quat[4]; - int gyro_status; - int accel_status; - int compass_status; - int nine_axis_status; -}; - -static struct hal_output_t hal_out; - -/** Acceleration (m/s^2) in body frame. -* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it -* should return a vector of magnitude near 9.81 m/s^2 -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_accel(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - int status; - /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. - * So this 9.80665 / 2^16 */ -#define ACCEL_CONVERSION 0.000149637603759766f - long accel[3]; - inv_get_accel_set(accel, accuracy, timestamp); - values[0] = accel[0] * ACCEL_CONVERSION; - values[1] = accel[1] * ACCEL_CONVERSION; - values[2] = accel[2] * ACCEL_CONVERSION; - if (hal_out.accel_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - -/** Linear Acceleration (m/s^2) in Body Frame. -* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show -* accel biases while at rest. -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_accel(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - long gravity[3], accel[3]; - - inv_get_accel_set(accel, accuracy, timestamp); - inv_get_gravity(gravity); - accel[0] -= gravity[0] >> 14; - accel[1] -= gravity[1] >> 14; - accel[2] -= gravity[2] >> 14; - values[0] = accel[0] * ACCEL_CONVERSION; - values[1] = accel[1] * ACCEL_CONVERSION; - values[2] = accel[2] * ACCEL_CONVERSION; - - return hal_out.nine_axis_status; -} - -/** Gravity vector (m/s^2) in Body Frame. -* @param[out] values Gravity vector in body frame, length 3, (m/s^2) -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_accel(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - long gravity[3]; - int status; - - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - inv_get_gravity(gravity); - values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; - values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; - values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; - if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) - status = 1; - else - status = 0; - return status; -} - -/** Gyroscope data (rad/s) in body frame. -* @param[out] values Rotation Rate in rad/sec. -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to -* inv_build_gyro(). -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. - * So this is: pi / 2^16 / 180 */ -#define GYRO_CONVERSION 2.66316109007924e-007f - long gyro[3]; - int status; - - inv_get_gyro_set(gyro, accuracy, timestamp); - values[0] = gyro[0] * GYRO_CONVERSION; - values[1] = gyro[1] * GYRO_CONVERSION; - values[2] = gyro[2] * GYRO_CONVERSION; - if (hal_out.gyro_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - -/** -* This corresponds to Sensor.TYPE_ROTATION_VECTOR. -* The rotation vector represents the orientation of the device as a combination -* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ -* around an axis {x, y, z}. <br> -* The three elements of the rotation vector are -* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation -* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is -* equal to the direction of the axis of rotation. -* -* The three elements of the rotation vector are equal to the last three components of a unit quaternion -* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2). -* -* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. -* The reference coordinate system is defined as a direct orthonormal basis, where: - - -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). - -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. - -Z points towards the sky and is perpendicular to the ground. -* @param[out] values Length 4. -* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate -* @param[out] timestamp Timestamp. In (ns) for Android. -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - - if (hal_out.nav_quat[0] >= 0) { - values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; - values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; - values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; - values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; - } else { - values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; - values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; - values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; - values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; - } - values[4] = inv_get_heading_confidence_interval(); - - return hal_out.nine_axis_status; -} - - -/** Compass data (uT) in body frame. -* @param[out] values Compass data in (uT), length 3. May be calibrated by having -* biases removed and sensitivity adjusted -* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate -* @param[out] timestamp Timestamp. In (ns) for Android. -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - int status; - /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. - * So this is: 1 / 2^16*/ -#define COMPASS_CONVERSION 1.52587890625e-005f - long compass[3]; - inv_get_compass_set(compass, accuracy, timestamp); - values[0] = compass[0] * COMPASS_CONVERSION; - values[1] = compass[1] * COMPASS_CONVERSION; - values[2] = compass[2] * COMPASS_CONVERSION; - if (hal_out.compass_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - - -/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. -* @param[out] values Length 3, Degrees.<br> -* - values[0]: Azimuth, angle between the magnetic north direction -* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br> -* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values -* when the z-axis moves toward the y-axis.<br> -* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive -* values when the x-axis moves toward the z-axis.<br> -* -* @note This definition is different from yaw, pitch and roll used in aviation -* where the X axis is along the long side of the plane (tail to nose). -* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() -* in conjunction with remapCoordinateSystem() and getOrientation() to compute -* these values instead. -* Important note: For historical reasons the roll angle is positive in the -* clockwise direction (mathematically speaking, it should be positive in -* the counter-clockwise direction). -* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. -* @param[out] timestamp The timestamp for this sensor. -* @return Returns 1 if the data was updated or 0 if it was not updated. -*/ -int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, - inv_time_t * timestamp) -{ - long t1, t2, t3; - long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; - - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - - q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]); - q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]); - q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]); - q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]); - q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]); - q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]); - q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]); - q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]); - q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]); - q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]); - - /* X component of the Ybody axis in World frame */ - t1 = q12 - q03; - - /* Y component of the Ybody axis in World frame */ - t2 = q22 + q00 - (1L << 30); - values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; - if (values[0] < 0) - values[0] += 360; - - /* Z component of the Ybody axis in World frame */ - t3 = q23 + q01; - values[1] = - -atan2f((float) t3, - sqrtf((float) t1 * t1 + - (float) t2 * t2)) * 180.f / (float) M_PI; - /* Z component of the Zbody axis in World frame */ - t2 = q33 + q00 - (1L << 30); - if (t2 < 0) { - if (values[1] >= 0) - values[1] = 180.f - values[1]; - else - values[1] = -180.f - values[1]; - } - - /* X component of the Xbody axis in World frame */ - t1 = q11 + q00 - (1L << 30); - /* Y component of the Xbody axis in World frame */ - t2 = q12 + q03; - /* Z component of the Xbody axis in World frame */ - t3 = q13 - q02; - //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI; - - values[2] = - -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) * - 180.f / (float) M_PI - 90); - if (values[2] >= 90) - values[2] = 180 - values[2]; - - if (values[2] < -90) - values[2] = -180 - values[2]; - - return hal_out.nine_axis_status; -} - -/** Main callback to generate HAL outputs. Typically not called by library users. -* @param[in] sensor_cal Input variable to take sensor data whenever there is new -* sensor data. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) -{ - int use_sensor = 0; - long sr; - (void) sensor_cal; - inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag, - &hal_out.nav_timestamp); - hal_out.gyro_status = sensor_cal->gyro.status; - hal_out.accel_status = sensor_cal->accel.status; - hal_out.compass_status = sensor_cal->compass.status; - - // Find the highest sample rate and tie generating 9-axis to that one. - if (sensor_cal->gyro.status & INV_SENSOR_ON) { - sr = sensor_cal->gyro.sample_rate_ms; - use_sensor = 0; - } - if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { - sr = sensor_cal->accel.sample_rate_ms; - use_sensor = 1; - } - if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { - sr = sensor_cal->compass.sample_rate_ms; - use_sensor = 2; - } - if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { - sr = sensor_cal->quat.sample_rate_ms; - use_sensor = 3; - } - - switch (use_sensor) { - default: - case 0: - hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->gyro.timestamp; - break; - case 1: - hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->accel.timestamp; - break; - case 2: - hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->compass.timestamp; - break; - case 3: - hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; - hal_out.nav_timestamp = sensor_cal->quat.timestamp; - break; - } - - return INV_SUCCESS; -} - -/** Turns off generation of HAL outputs. -* @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_stop_hal_outputs(void) -{ - inv_error_t result; - result = inv_unregister_data_cb(inv_generate_hal_outputs); - return result; -} - -/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() -* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_start_hal_outputs(void) -{ - inv_error_t result; - result = - inv_register_data_cb(inv_generate_hal_outputs, - INV_PRIORITY_HAL_OUTPUTS, - INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); - return result; -} - -/** Initializes hal outputs class. This is called automatically by the -* enable function. It may be called any time the feature is enabled, but -* is typically not needed to be called by outside callers. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_init_hal_outputs(void) -{ - memset(&hal_out, 0, sizeof(hal_out)); - return INV_SUCCESS; -} - -/** Turns on creation and storage of HAL type results. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_enable_hal_outputs(void) -{ - inv_error_t result; - - // don't need to check the result for inv_init_hal_outputs - // since it's always INV_SUCCESS - inv_init_hal_outputs(); - - result = inv_register_mpl_start_notification(inv_start_hal_outputs); - return result; -} - -/** Turns off creation and storage of HAL type results. -*/ -inv_error_t inv_disable_hal_outputs(void) -{ - inv_error_t result; - - inv_stop_hal_outputs(); // Ignore error if we have already stopped this - result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); - return result; -} - -/** - * @} - */ diff --git a/libsensors/software/core/mllite/hal_outputs.h b/libsensors/software/core/mllite/hal_outputs.h deleted file mode 100644 index ed4cb65..0000000 --- a/libsensors/software/core/mllite/hal_outputs.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_HAL_OUTPUTS_H__ -#define INV_HAL_OUTPUTS_H__ - -#ifdef __cplusplus -extern "C" { -#endif - - int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, - inv_time_t * timestamp); - - int inv_get_sensor_type_linear_acceleration(float *values, - int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, - inv_time_t * timestamp); - - inv_error_t inv_enable_hal_outputs(void); - inv_error_t inv_disable_hal_outputs(void); - inv_error_t inv_init_hal_outputs(void); - inv_error_t inv_start_hal_outputs(void); - inv_error_t inv_stop_hal_outputs(void); - -#ifdef __cplusplus -} -#endif - -#endif // INV_HAL_OUTPUTS_H__ diff --git a/libsensors/software/core/mllite/invensense.h b/libsensors/software/core/mllite/invensense.h deleted file mode 100644 index fb8e12b..0000000 --- a/libsensors/software/core/mllite/invensense.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -/** - * Main header file for Invensense's basic library. - */ - -#include "data_builder.h" -#include "hal_outputs.h" -#include "message_layer.h" -#include "mlmath.h" -#include "ml_math_func.h" -#include "mpl.h" -#include "results_holder.h" -#include "start_manager.h" -#include "storage_manager.h" -#include "log.h" -#include "mlinclude.h" -//#include "..\HAL\include\mlos.h" diff --git a/libsensors/software/core/mllite/linux/inv_sysfs_utils.c b/libsensors/software/core/mllite/linux/inv_sysfs_utils.c deleted file mode 100644 index 649b917..0000000 --- a/libsensors/software/core/mllite/linux/inv_sysfs_utils.c +++ /dev/null @@ -1,318 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#include <string.h> -#include <stdlib.h> -#include <ctype.h> -#include <stdio.h> -#include <stdint.h> -#include <dirent.h> -#include <errno.h> -#include <unistd.h> -#include "inv_sysfs_utils.h" - -/* General TODO list: - * Select more reasonable string lengths or use fseek and malloc. - */ - -/** - * inv_sysfs_write() - Write an integer to a file. - * @filename: Path to file. - * @data: Value to write to file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_write(char *filename, long data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; -} - -/** - * inv_sysfs_read() - Read a string from a file. - * @filename: Path to file. - * @num_bytes: Number of bytes to read. - * @data: Data from file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_read(char *filename, long num_bytes, char *data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "r"); - if (!fp) - return -errno; - count = fread(data, 1, num_bytes, fp); - fclose(fp); - return count; -} - -/** - * inv_read_buffer() - Read data from ring buffer. - * @fd: File descriptor for buffer file. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_buffer(int fd, long *data, long long *timestamp) -{ - char str[35]; - int count; - - count = read(fd, str, sizeof(str)); - if (!count) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_raw() - Read raw data. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - char str[40]; - int count; - - count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str); - if (count < 0) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_temperature_raw() - Read temperature. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp) -{ - char str[25]; - int count; - - count = inv_sysfs_read((char*)names->temperature, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd%lld", &data[0], timestamp); - if (count < 2) - return -EAGAIN; - return count; -} - -/** - * inv_read_fifo_rate() - Read fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data) -{ - char str[8]; - int count; - - count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_power_state() - Read power state. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data) -{ - char str[2]; - int count; - - count = inv_sysfs_read((char*)names->power_state, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", (short*)data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_scale() - Read scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data) -{ - char str[5]; - int count; - - count = inv_sysfs_read((char*)names->scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%f", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_scale() - Read temperature scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_offset() - Read temperature offset. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_q16() - Get data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count; - short raw[3]; - float scale; - count = inv_read_raw(names, (long*)raw, timestamp); - count += inv_read_scale(names, &scale); - data[0] = (long)(raw[0] * (65536.f / scale)); - data[1] = (long)(raw[1] * (65536.f / scale)); - data[2] = (long)(raw[2] * (65536.f / scale)); - return count; -} - -/** - * inv_read_q16() - Get temperature data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes read or a negative error code. - */ -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count = 0; - short raw; - static short scale, offset; - static unsigned char first_read = 1; - - if (first_read) { - count += inv_read_temp_scale(names, &scale); - count += inv_read_temp_offset(names, &offset); - first_read = 0; - } - count += inv_read_temperature_raw(names, &raw, timestamp); - data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f); - - return count; -} - -/** - * inv_write_fifo_rate() - Write fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data) -{ - return inv_sysfs_write((char*)names->fifo_rate, (long)data); -} - -/** - * inv_write_buffer_enable() - Enable/disable buffer in /dev. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->enable, (long)data); -} - -/** - * inv_write_power_state() - Turn device on/off. - * @names: Names of sysfs files. - * @data: 1 to turn on. - * Returns number of bytes written or a negative error code. - */ -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->power_state, (long)data); -} - - - diff --git a/libsensors/software/core/mllite/linux/inv_sysfs_utils.h b/libsensors/software/core/mllite/linux/inv_sysfs_utils.h deleted file mode 100644 index 45a35f9..0000000 --- a/libsensors/software/core/mllite/linux/inv_sysfs_utils.h +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#ifndef _INV_SYSFS_UTILS_H_ -#define _INV_SYSFS_UTILS_H_ - -/** - * struct inv_sysfs_names_s - Files needed by user applications. - * @buffer: Ring buffer attached to FIFO. - * @enable: Turns on HW-to-ring buffer flow. - * @raw_data: Raw data from registers. - * @temperature: Temperature data from register. - * @fifo_rate: FIFO rate/ODR. - * @power_state: Power state (this is a five-star comment). - * @fsr: Full-scale range. - * @lpf: Digital low pass filter. - * @scale: LSBs / dps (or LSBs / Gs). - * @temp_scale: LSBs / degrees C. - * @temp_offset: Offset in LSBs. - */ -struct inv_sysfs_names_s { - - //Sysfs for ITG3500 & MPU6050 - const char *buffer; - const char *enable; - const char *raw_data; //Raw Gyro data - const char *temperature; - const char *fifo_rate; - const char *power_state; - const char *fsr; - const char *lpf; - const char *scale; //Gyro scale - const char *temp_scale; - const char *temp_offset; - const char *self_test; - //Starting Sysfs available for MPU6050 only - const char *accel_en; - const char *accel_fifo_en; - const char *accel_fs; - const char *clock_source; - const char *early_suspend_en; - const char *firmware_loaded; - const char *gyro_en; - const char *gyro_fifo_en; - const char *key; - const char *raw_accel; - const char *reg_dump; - const char *tap_on; - const char *dmp_firmware; -}; - -/* File IO. Typically won't be called directly by user application, but they'll - * be here for your enjoyment. - */ -int inv_sysfs_write(char *filename, long data); -int inv_sysfs_read(char *filename, long num_bytes, char *data); - -/* Helper APIs to extract specific data. */ -int inv_read_buffer(int fd, long *data, long long *timestamp); -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp); -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data); -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data); -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data); -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data); -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data); -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data); -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data); -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data); - -/* Scaled data. */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); - - -#endif /* #ifndef _INV_SYSFS_UTILS_H_ */ - - diff --git a/libsensors/software/core/mllite/linux/ml_load_dmp.c b/libsensors/software/core/mllite/linux/ml_load_dmp.c deleted file mode 100644 index f0f078c..0000000 --- a/libsensors/software/core/mllite/linux/ml_load_dmp.c +++ /dev/null @@ -1,281 +0,0 @@ -/* - $License: - Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id:$ - * - *****************************************************************************/ - -/** - * @defgroup ML_LOAD_DMP - * - * @{ - * @file ml_load_dmp.c - * @brief functions for writing dmp firmware. - */ -#include <stdio.h> - -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-loaddmp" - -#include "ml_load_dmp.h" -#include "log.h" -#include "mlos.h" - -#define LOADDMP_LOG MPL_LOGI -#define LOADDMP_LOG MPL_LOGI - -#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) -#define DMP_CODE_SIZE 3060 - -static const unsigned char dmpMemory[DMP_CODE_SIZE] = { - /* bank # 0 */ - 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, - 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, - 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, - 0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, - 0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, - 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, - 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, - 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, - 0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, - 0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, - /* bank # 1 */ - 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, - 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, - 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, - 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, - 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, - 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, - 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, - 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, - 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, - /* bank # 2 */ - 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x44, 0x00, 0x05, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xc0, 0x00, 0x00, 0x01, 0x3f, 0xff, 0xff, 0xff, 0x40, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, - 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, - 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, - 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - /* bank # 3 */ - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, - 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - /* bank # 4 */ - 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, - 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, - 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, - 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, 0xca, 0xf1, - 0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, 0x99, 0x2c, - 0x54, 0x7c, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, - 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, - 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, - 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, - 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, - 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, - 0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, - 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, - 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, - 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, - 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, - /* bank # 5 */ - 0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, - 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, - 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, - 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, - 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, - 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, - 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, - 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, - 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, - 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, - 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, - 0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, - 0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, - 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, - 0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, - 0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, - /* bank # 6 */ - 0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, - 0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, - 0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, - 0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, - 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, - 0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, - 0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, - 0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, - 0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, - 0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, - 0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, - 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, - 0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, - 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, - 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, - 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, - /* bank # 7 */ - 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, - 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, - 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, - 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, - 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, - 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, - 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, - 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, - 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, - 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, - 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, - 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, - 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, - 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, - 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, - 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, - /* bank # 8 */ - 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, - 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, - 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, - 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, - 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, - 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, - 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 0xb4, 0xb0, - 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, - 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, - 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, - 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, - 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, - 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, - 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, - 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, - 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, - /* bank # 9 */ - 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, - 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, - 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, - 0x88, 0x2d, 0x55, 0x7d, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, - 0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, - 0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, - 0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, - 0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, - 0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, - 0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, - 0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, - 0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, - 0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, - 0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, - 0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, - 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, - /* bank # 10 */ - 0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, - 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, - 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, - 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, - 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, - 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, - 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, - 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x8b, 0x93, 0xf2, 0x02, 0x02, 0xd1, 0xab, 0xda, - 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, - 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, - 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, - 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 0xa3, 0xa3, - 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, - 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 0xb4, 0x80, - 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 0x2c, 0x87, - 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 0x51, 0x79, - /* bank # 11 */ - 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 0x90, 0x28, - 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 0x80, 0xa2, - 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 0x80, 0xd9, - 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 0x2c, 0xd8, - 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 0xda, 0x3c, - 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 0x0c, 0xd8, - 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 0xff, 0xd8, - 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 0x99, 0xa8, - 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x87, 0x31, - 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0xa8, 0x82, - 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 0xf1, 0xa6, - 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 0xf2, 0x2a, - 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 0xac, 0x8c, - 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 0xac, 0xde, - 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 0xa6, 0xd9, - 0x00, 0xd8, 0xf1, 0xff -}; - -#define DMP_VERSION (dmpMemory) - -inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) -{ - inv_error_t result = INV_SUCCESS; - int bytesWritten = 0; - - if (len <= 0) { - MPL_LOGE("Nothing to write"); - return INV_ERROR_FILE_WRITE; - } - else { - MPL_LOGI("dmp firmware size to write = %d", len); - } - if ( fd == NULL ) { - return INV_ERROR_FILE_OPEN; - } - bytesWritten = fwrite(dmp, 1, len, fd); - if (bytesWritten != len) { - MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", - bytesWritten, len); - result = INV_ERROR_FILE_WRITE; - } - else { - MPL_LOGI("Bytes written = %d", bytesWritten); - } - return result; -} - -inv_error_t inv_load_dmp(FILE *fd) -{ - inv_error_t result = INV_SUCCESS; - result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE); - return result; -} - -/** - * @} - */ diff --git a/libsensors/software/core/mllite/linux/ml_load_dmp.h b/libsensors/software/core/mllite/linux/ml_load_dmp.h deleted file mode 100644 index 4d98692..0000000 --- a/libsensors/software/core/mllite/linux/ml_load_dmp.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - $License: - Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -#ifndef INV_LOAD_DMP_H -#define INV_LOAD_DMP_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Includes. -*/ -#include "mltypes.h" - -/* - APIs -*/ -inv_error_t inv_load_dmp(FILE *fd); - -#ifdef __cplusplus -} -#endif -#endif /* INV_LOAD_DMP_H */ diff --git a/libsensors/software/core/mllite/linux/ml_stored_data.c b/libsensors/software/core/mllite/linux/ml_stored_data.c deleted file mode 100644 index c5cf2e6..0000000 --- a/libsensors/software/core/mllite/linux/ml_stored_data.c +++ /dev/null @@ -1,353 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup ML_STORED_DATA - * - * @{ - * @file ml_stored_data.c - * @brief functions for reading and writing stored data sets. - * Typically, these functions process stored calibration data. - */ - -#include <stdio.h> - -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-storeload" - - -#include "ml_stored_data.h" -#include "storage_manager.h" -#include "log.h" -#include "mlos.h" - -#define LOADCAL_DEBUG 0 -#define STORECAL_DEBUG 0 - -#define DEFAULT_KEY 29681 - -#define STORECAL_LOG MPL_LOGI -#define LOADCAL_LOG MPL_LOGI - -inv_error_t inv_read_cal(unsigned char *cal, size_t len) -{ - FILE *fp; - int bytesRead; - inv_error_t result = INV_SUCCESS; - - fp = fopen(MLCAL_FILE,"rb"); - if (fp == NULL) { - MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - bytesRead = fread(cal, 1, len, fp); - if (bytesRead != len) { - MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", - bytesRead, len); - result = INV_ERROR_FILE_READ; - goto read_cal_end; - } - else { - MPL_LOGI("Bytes read = %d", bytesRead); - } - -read_cal_end: - fclose(fp); - return result; -} - -inv_error_t inv_write_cal(unsigned char *cal, size_t len) -{ - FILE *fp; - int bytesWritten; - inv_error_t result = INV_SUCCESS; - - if (len <= 0) { - MPL_LOGE("Nothing to write"); - return INV_ERROR_FILE_WRITE; - } - else { - MPL_LOGI("cal data size to write = %d", len); - } - fp = fopen(MLCAL_FILE,"wb"); - if (fp == NULL) { - MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - bytesWritten = fwrite(cal, 1, len, fp); - if (bytesWritten != len) { - MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", - bytesWritten, len); - result = INV_ERROR_FILE_WRITE; - } - else { - MPL_LOGI("Bytes written = %d", bytesWritten); - } - fclose(fp); - return result; -} - -/** - * @brief Loads a type 0 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature compensation : temperature data points, - * - temperature compensation : gyro biases data points for X, Y, - * and Z axes. - * - accel biases for X, Y, Z axes. - * This calibration data is produced internally by the MPL and its - * size is 2777 bytes (header and checksum included). - * Calibration format type 1 is currently used for ITG3500 - * - * @pre inv_init_storage_manager() - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) -{ - inv_error_t result; - - LOADCAL_LOG("Entering inv_load_cal_V0\n"); - - /*if (len != expLen) { - MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", - expLen, len); - return INV_ERROR_FILE_READ; - }*/ - - result = inv_load_mpl_states(calData, len); - return result; -} - -/** - * @brief Loads a type 1 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature, - * - gyro biases for X, Y, Z axes, - * - accel biases for X, Y, Z axes. - * This calibration data would normally be produced by the MPU Self - * Test and its size is 36 bytes (header and checksum included). - * Calibration format type 1 is produced by the MPU Self Test and - * substitutes the type 0 : inv_load_cal_V0(). - * - * @pre - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) -{ - return INV_SUCCESS; -} - -/** - * @brief Loads a set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * - * @pre - * - * - * @param calData - * A pointer to an array of bytes to be parsed. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal(unsigned char *calData) -{ - int calType = 0; - int len = 0; - //int ptr; - //uint32_t chk = 0; - //uint32_t cmp_chk = 0; - - /*load_func_t loaders[] = { - inv_load_cal_V0, - inv_load_cal_V1, - }; - */ - - inv_load_cal_V0(calData, len); - - /* read the header (type and len) - len is the total record length including header and checksum */ - len = 0; - len += 16777216L * ((int)calData[0]); - len += 65536L * ((int)calData[1]); - len += 256 * ((int)calData[2]); - len += (int)calData[3]; - - calType = ((int)calData[4]) * 256 + ((int)calData[5]); - if (calType > 5) { - MPL_LOGE("Unsupported calibration file format %d. " - "Valid types 0..5\n", calType); - return INV_ERROR_INVALID_PARAMETER; - } - - /* call the proper method to read in the data */ - //return loaders[calType] (calData, len); - return 0; -} - -/** - * @brief Stores a set of calibration data. - * It generates a binary data set containing calibration data. - * The binary data set is intended to be stored into a file. - * - * @pre inv_dmp_open() - * - * @param calData - * A pointer to an array of bytes to be stored. - * @param length - * The amount of bytes available in the array. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_store_cal(unsigned char *calData, size_t length) -{ - inv_error_t res = 0; - size_t size; - - STORECAL_LOG("Entering inv_store_cal\n"); - - inv_get_mpl_state_size(&size); - - MPL_LOGI("inv_get_mpl_state_size() : size=%d", size); - - /* store data */ - res = inv_save_mpl_states(calData, size); - if(res != 0) - { - MPL_LOGE("inv_save_mpl_states() failed"); - } - - STORECAL_LOG("Exiting inv_store_cal\n"); - return INV_SUCCESS; -} - -/** - * @brief Load a calibration file. - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return 0 or error code. - */ -inv_error_t inv_load_calibration(void) -{ - unsigned char *calData; - inv_error_t result = 0; - size_t length; - - inv_get_mpl_state_size(&length); - if (length <= 0) { - MPL_LOGE("Could not get file calibration length - " - "error %d - aborting\n", result); - return result; - } - - calData = (unsigned char *)inv_malloc(length); - if (!calData) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", length); - return INV_ERROR_MEMORY_EXAUSTED; - } - - result = inv_read_cal(calData, length); - if(result != INV_SUCCESS) { - MPL_LOGE("Could not load cal file - " - "aborting\n"); - } - - result = inv_load_mpl_states(calData, length); - if (result != INV_SUCCESS) { - MPL_LOGE("Could not load the calibration data - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - } - -free_mem_n_exit: - inv_free(calData); - return INV_SUCCESS; -} - -/** - * @brief Store runtime calibration data to a file - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return 0 or error code. - */ -inv_error_t inv_store_calibration(void) -{ - unsigned char *calData; - inv_error_t result; - size_t length; - - result = inv_get_mpl_state_size(&length); - calData = (unsigned char *)inv_malloc(length); - if (!calData) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", length); - return INV_ERROR_MEMORY_EXAUSTED; - } - else { - MPL_LOGI("mpl state size = %d", length); - } - - result = inv_save_mpl_states(calData, length); - if (result != INV_SUCCESS) { - MPL_LOGE("Could not save mpl states - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - } - else { - MPL_LOGE("calData from inv_save_mpl_states, size=%d", - strlen((char *)calData)); - } - - result = inv_write_cal(calData, length); - if (result != INV_SUCCESS) { - MPL_LOGE("Could not store calibrated data on file - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - - } - -free_mem_n_exit: - inv_free(calData); - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/libsensors/software/core/mllite/linux/ml_stored_data.h b/libsensors/software/core/mllite/linux/ml_stored_data.h deleted file mode 100644 index 336f081..0000000 --- a/libsensors/software/core/mllite/linux/ml_stored_data.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $ - * - ******************************************************************************/ - -#ifndef INV_MPL_STORED_DATA_H -#define INV_MPL_STORED_DATA_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Includes. -*/ -#include "mltypes.h" - -/* - Defines -*/ -#define MLCAL_FILE "/data/inv_cal_data.bin" - -/* - APIs -*/ -inv_error_t inv_load_calibration(void); -inv_error_t inv_store_calibration(void); - -/* - Internal APIs -*/ -inv_error_t inv_read_cal(unsigned char *cal, size_t len); -inv_error_t inv_write_cal(unsigned char *cal, size_t len); -inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len); -inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len); - -/* - Other prototypes -*/ -inv_error_t inv_load_cal(unsigned char *calData); -inv_error_t inv_store_cal(unsigned char *calData, size_t length); - -#ifdef __cplusplus -} -#endif -#endif /* INV_MPL_STORED_DATA_H */ diff --git a/libsensors/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors/software/core/mllite/linux/ml_sysfs_helper.c deleted file mode 100644 index 5636a02..0000000 --- a/libsensors/software/core/mllite/linux/ml_sysfs_helper.c +++ /dev/null @@ -1,416 +0,0 @@ -#include <string.h> -#include <stdio.h> -#include "ml_sysfs_helper.h" -#include <dirent.h> -#include <ctype.h> -#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" - -#define CHIP_NUM 4 -enum PROC_SYSFS_CMD { - CMD_GET_SYSFS_PATH, - CMD_GET_DMP_PATH, - CMD_GET_CHIP_NAME, - CMD_GET_SYSFS_KEY, - CMD_GET_TRIGGER_PATH, - CMD_GET_DEVICE_NODE -}; -static char sysfs_path[100]; -static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"}; -static int chip_ind; -static int initialized =0; -static int status = 0; -static int iio_initialized = 0; -static int iio_dev_num = 0; - -#define IIO_MAX_NAME_LENGTH 30 - -#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" -#define FORMAT_TYPE_FILE "%s_type" - -static const char *iio_dir = "/sys/bus/iio/devices/"; - -/** - * find_type_by_name() - function to match top level types by name - * @name: top level type instance name - * @type: the type of top level instance being sort - * - * Typical types this is used for are device and trigger. - **/ -int find_type_by_name(const char *name, const char *type) -{ - const struct dirent *ent; - int number, numstrlen; - - FILE *nameFile; - DIR *dp; - char thisname[IIO_MAX_NAME_LENGTH]; - char *filename; - - dp = opendir(iio_dir); - if (dp == NULL) { - printf("No industrialio devices available"); - return -ENODEV; - } - - while (ent = readdir(dp), ent != NULL) { - if (strcmp(ent->d_name, ".") != 0 && - strcmp(ent->d_name, "..") != 0 && - strlen(ent->d_name) > strlen(type) && - strncmp(ent->d_name, type, strlen(type)) == 0) { - numstrlen = sscanf(ent->d_name + strlen(type), - "%d", - &number); - /* verify the next character is not a colon */ - if (strncmp(ent->d_name + strlen(type) + numstrlen, - ":", - 1) != 0) { - filename = malloc(strlen(iio_dir) - + strlen(type) - + numstrlen - + 6); - if (filename == NULL) - return -ENOMEM; - sprintf(filename, "%s%s%d/name", - iio_dir, - type, - number); - nameFile = fopen(filename, "r"); - if (!nameFile) - continue; - free(filename); - fscanf(nameFile, "%s", thisname); - if (strcmp(name, thisname) == 0) - return number; - fclose(nameFile); - } - } - } - return -ENODEV; -} - -/* mode 0: search for which chip in the system and fill sysfs path - mode 1: return event number - */ -static int parsing_proc_input(int mode, char *name){ - const char input[] = "/proc/bus/input/devices"; - char line[100], d; - char tmp[100]; - FILE *fp; - int i, j, result, find_flag; - int event_number = -1; - int input_number = -1; - - if(NULL == (fp = fopen(input, "rt")) ){ - return -1; - } - result = 1; - find_flag = 0; - while(result != 0 && find_flag < 2){ - i = 0; - d = 0; - memset(line, 0, 100); - while(d != '\n'){ - result = fread(&d, 1, 1, fp); - if(result == 0){ - line[0] = 0; - break; - } - sprintf(&line[i], "%c", d); - i ++; - } - if(line[0] == 'N'){ - i = 1; - while(line[i] != '"'){ - i++; - } - i++; - j = 0; - find_flag = 0; - if (mode == 0){ - while(j < CHIP_NUM){ - if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){ - find_flag = 1; - chip_ind = j; - } - j++; - } - } else if (mode != 0){ - if(!memcmp(&line[i], name, strlen(name))){ - find_flag = 1; - } - } - } - if(find_flag){ - if(mode == 0){ - if(line[0] == 'S'){ - memset(tmp, 0, 100); - i =1; - while(line[i] != '=') i++; - i++; - j = 0; - while(line[i] != '\n'){ - tmp[j] = line[i]; - i ++; j++; - } - sprintf(sysfs_path, "%s%s", "/sys", tmp); - find_flag++; - } - } else if(mode == 1){ - if(line[0] == 'H') { - i = 2; - while(line[i] != '=') i++; - while(line[i] != 't') i++; - i++; - event_number = 0; - while(line[i] != '\n'){ - if(line[i] >= '0' && line[i] <= '9') - event_number = event_number*10 + line[i]-0x30; - i ++; - } - find_flag ++; - } - } else if (mode == 2) { - if(line[0] == 'S'){ - memset(tmp, 0, 100); - i =1; - while(line[i] != '=') i++; - i++; - j = 0; - while(line[i] != '\n'){ - tmp[j] = line[i]; - i ++; j++; - } - input_number = 0; - if(tmp[j-2] >= '0' && tmp[j-2] <= '9') - input_number += (tmp[j-2]-0x30)*10; - if(tmp[j-1] >= '0' && tmp[j-1] <= '9') - input_number += (tmp[j-1]-0x30); - find_flag++; - } - } - } - } - fclose(fp); - if(find_flag == 0){ - return -1; - } - if(0 == mode) - status = 1; - if (mode == 1) - return event_number; - if (mode == 2) - return input_number; - return 0; - -} -static void init_iio() { - int i, j; - char iio_chip[10]; - int dev_num; - for(j=0; j< CHIP_NUM; j++) { - for (i=0; i<strlen(chip_name[j]); i++) { - iio_chip[i] = tolower(chip_name[j][i]); - } - iio_chip[strlen(chip_name[0])] = '\0'; - dev_num = find_type_by_name(iio_chip, "iio:device"); - if(dev_num >= 0) { - iio_initialized = 1; - iio_dev_num = dev_num; - chip_ind = j; - } - } -} - -static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data) -{ - char key_path[100]; - FILE *fp; - int i, result; - if(initialized == 0){ - parsing_proc_input(0, NULL); - initialized = 1; - } - if(initialized && status == 0) { - init_iio(); - if (iio_initialized == 0) - return -1; - } - - memset(key_path, 0, 100); - switch(cmd){ - case CMD_GET_SYSFS_PATH: - if (iio_initialized == 1) - sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num); - else - sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu"); - break; - case CMD_GET_DMP_PATH: - if (iio_initialized == 1) - sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num); - else - sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware"); - break; - case CMD_GET_CHIP_NAME: - sprintf(data, "%s", chip_name[chip_ind]); - break; - case CMD_GET_TRIGGER_PATH: - sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num); - break; - case CMD_GET_DEVICE_NODE: - sprintf(data, "/dev/iio:device%d", iio_dev_num); - break; - case CMD_GET_SYSFS_KEY: - memset(key_path, 0, 100); - if (iio_initialized == 1) - sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num); - else - sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key"); - - if((fp = fopen(key_path, "rt")) == NULL) - return -1; - for(i=0;i<16;i++){ - fscanf(fp, "%02x", &result); - data[i] = (char)result; - } - - fclose(fp); - break; - default: - break; - } - return 0; -} -/** - * @brief return sysfs key. if the key is not available - * return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the key - * It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_sysfs_key(unsigned char *key) -{ - if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -/** - * @brief return the sysfs path. If the path is not - * found yet. return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the sysfs - * path. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_sysfs_path(char *name) -{ - if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -inv_error_t inv_get_sysfs_abs_path(char *name) -{ - strcpy(name, MPU_SYSFS_ABS_PATH); - return INV_SUCCESS; -} - -/** - * @brief return the dmp file path. If the path is not - * found yet. return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the dmp file - * path. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_dmpfile(char *name) -{ - if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} -/** - * @brief return the chip name. If the chip is not - * found yet. return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the chip name - * path(8 bytes). It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_chip_name(char *name) -{ - if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} -/** - * @brief return event handler number. If the handler number is not found - * return false. the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the chip name - * path(8 bytes). It should be zeroed before calling this function. - * Or it could have unpredicable result. - * @int *num: event number store - */ -inv_error_t inv_get_handler_number(const char *name, int *num) -{ - initialized = 0; - if ((*num = parsing_proc_input(1, (char *)name)) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -/** - * @brief return input number. If the handler number is not found - * return false. the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the chip name - * path(8 bytes). It should be zeroed before calling this function. - * Or it could have unpredicable result. - * @int *num: input number store - */ -inv_error_t inv_get_input_number(const char *name, int *num) -{ - initialized = 0; - if ((*num = parsing_proc_input(2, (char *)name)) < 0) - return INV_ERROR_NOT_OPENED; - else { - return INV_SUCCESS; - } -} - -/** - * @brief return iio trigger name. If iio is not initialized, return false. - * So the return must be checked to make sure the numeber is valid. - * @unsigned char *name: This should be array big enough to hold the trigger - * name. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_iio_trigger_path(const char *name) -{ - if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -/** - * @brief return iio device node. If iio is not initialized, return false. - * So the return must be checked to make sure the numeber is valid. - * @unsigned char *name: This should be array big enough to hold the device - * node. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_iio_device_node(const char *name) -{ - if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} diff --git a/libsensors/software/core/mllite/linux/ml_sysfs_helper.h b/libsensors/software/core/mllite/linux/ml_sysfs_helper.h deleted file mode 100644 index eb285c5..0000000 --- a/libsensors/software/core/mllite/linux/ml_sysfs_helper.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id$ - * - ******************************************************************************/ - -#ifndef MLDMP_SYSFS_HELPER_H__ -#define MLDMP_SYSFS_HELPER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#include "invensense.h" - -int find_type_by_name(const char *name, const char *type); -inv_error_t inv_get_sysfs_path(char *name); -inv_error_t inv_get_sysfs_abs_path(char *name); -inv_error_t inv_get_dmpfile(char *name); -inv_error_t inv_get_chip_name(char *name); -inv_error_t inv_get_sysfs_key(unsigned char *key); -inv_error_t inv_get_handler_number(const char *name, int *num); -inv_error_t inv_get_input_number(const char *name, int *num); -inv_error_t inv_get_iio_trigger_path(const char *name); -inv_error_t inv_get_iio_device_node(const char *name); - -#ifdef __cplusplus -} -#endif -#endif /* MLDMP_SYSFS_HELPER_H__ */ diff --git a/libsensors/software/core/mllite/linux/mlos.h b/libsensors/software/core/mllite/linux/mlos.h deleted file mode 100644 index 287025f..0000000 --- a/libsensors/software/core/mllite/linux/mlos.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef _MLOS_H -#define _MLOS_H - -#ifndef __KERNEL__ -#include <stdio.h> -#endif - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(LINUX) || defined(__KERNEL__) -typedef unsigned int HANDLE; -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /* - MLOSCreateFile defines. - */ - -#define MLOS_GENERIC_READ ((unsigned int)0x80000000) -#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) -#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) -#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) -#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) - - /* ---------- */ - /* - Enums. - */ - /* ---------- */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - -#ifndef __KERNEL__ -#include <string.h> - void *inv_malloc(unsigned int numBytes); - inv_error_t inv_free(void *ptr); - inv_error_t inv_create_mutex(HANDLE *mutex); - inv_error_t inv_lock_mutex(HANDLE mutex); - inv_error_t inv_unlock_mutex(HANDLE mutex); - FILE *inv_fopen(char *filename); - void inv_fclose(FILE *fp); - - inv_error_t inv_destroy_mutex(HANDLE handle); - - void inv_sleep(int mSecs); - unsigned long inv_get_tick_count(void); - - /* Kernel implmentations */ -#define GFP_KERNEL (0x70) - static inline void *kmalloc(size_t size, - unsigned int gfp_flags) - { - (void)gfp_flags; - return inv_malloc((unsigned int)size); - } - static inline void *kzalloc(size_t size, unsigned int gfp_flags) - { - void *tmp = inv_malloc((unsigned int)size); - (void)gfp_flags; - if (tmp) - memset(tmp, 0, size); - return tmp; - } - static inline void kfree(void *ptr) - { - inv_free(ptr); - } - static inline void msleep(long msecs) - { - inv_sleep(msecs); - } - static inline void udelay(unsigned long usecs) - { - inv_sleep((usecs + 999) / 1000); - } -#else -#include <linux/delay.h> -#endif - -#ifdef __cplusplus -} -#endif -#endif /* _MLOS_H */ diff --git a/libsensors/software/core/mllite/linux/mlos_linux.c b/libsensors/software/core/mllite/linux/mlos_linux.c deleted file mode 100644 index 75f062e..0000000 --- a/libsensors/software/core/mllite/linux/mlos_linux.c +++ /dev/null @@ -1,194 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/******************************************************************************* - * - * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup MLOS - * @brief OS Interface. - * - * @{ - * @file mlos.c - * @brief OS Interface. -**/ - -/* ------------- */ -/* - Includes. - */ -/* ------------- */ - -#include <sys/time.h> -#include <unistd.h> -#include <pthread.h> -#include <stdlib.h> - -#include "stdint_invensense.h" - -#include "mlos.h" -#include <errno.h> - - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Allocate space - * @param numBytes number of bytes - * @return pointer to allocated space -**/ -void *inv_malloc(unsigned int numBytes) -{ - // Allocate space. - void *allocPtr = malloc(numBytes); - return allocPtr; -} - - -/** - * @brief Free allocated space - * @param ptr pointer to space to deallocate - * @return error code. -**/ -inv_error_t inv_free(void *ptr) -{ - // Deallocate space. - free(ptr); - - return INV_SUCCESS; -} - - -/** - * @brief Mutex create function - * @param mutex pointer to mutex handle - * @return error code. - */ -inv_error_t inv_create_mutex(HANDLE *mutex) -{ - int res; - pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); - if(pm == NULL) - return INV_ERROR; - - res = pthread_mutex_init(pm, NULL); - if(res == -1) { - free(pm); - return INV_ERROR_OS_CREATE_FAILED; - } - - *mutex = (HANDLE)pm; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex lock function - * @param mutex Mutex handle - * @return error code. - */ -inv_error_t inv_lock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t*)mutex; - - res = pthread_mutex_lock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex unlock function - * @param mutex mutex handle - * @return error code. -**/ -inv_error_t inv_unlock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t*)mutex; - - res = pthread_mutex_unlock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief open file - * @param filename name of the file to open. - * @return error code. - */ -FILE *inv_fopen(char *filename) -{ - FILE *fp = fopen(filename,"r"); - return fp; -} - - -/** - * @brief close the file. - * @param fp handle to file to close. - * @return error code. - */ -void inv_fclose(FILE *fp) -{ - fclose(fp); -} - -/** - * @brief Close Handle - * @param handle handle to the resource. - * @return Zero if success, an error code otherwise. - */ -inv_error_t inv_destroy_mutex(HANDLE handle) -{ - int error; - pthread_mutex_t *pm = (pthread_mutex_t*)handle; - error = pthread_mutex_destroy(pm); - if (error) { - return errno; - } - free((void*) handle); - - return INV_SUCCESS;} - - -/** - * @brief Sleep function. - */ -void inv_sleep(int mSecs) -{ - usleep(mSecs*1000); -} - - -/** - * @brief get system's internal tick count. - * Used for time reference. - * @return current tick count. - */ -unsigned long inv_get_tick_count() -{ - struct timeval tv; - - if (gettimeofday(&tv, NULL) !=0) - return 0; - - return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); -} - - /**********************/ - /** @} */ /* defgroup */ -/**********************/ - diff --git a/libsensors/software/core/mllite/message_layer.c b/libsensors/software/core/mllite/message_layer.c deleted file mode 100644 index 8317957..0000000 --- a/libsensors/software/core/mllite/message_layer.c +++ /dev/null @@ -1,59 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup Message_Layer message_layer
- * @brief Motion Library - Message Layer
- * Holds Low Occurance messages
- *
- * @{
- * @file message_layer.c
- * @brief Holds Low Occurance Messages.
- */
-#include "message_layer.h"
-#include "log.h"
-
-struct message_holder_t {
- long message;
-};
-
-static struct message_holder_t mh;
-
-/** Sets a message.
-* @param[in] set The flags to set.
-* @param[in] clear Before setting anything this will clear these messages,
-* which is useful for mutually exclusive messages such
-* a motion or no motion message.
-* @param[in] level Level of the messages. It starts at 0, and may increase
-* in the future to allow more messages if the bit storage runs out.
-*/
-void inv_set_message(long set, long clear, int level)
-{
- if (level == 0) {
- mh.message &= ~clear;
- mh.message |= set;
- }
-}
-
-/** Returns Message Flags for Level 0 Messages.
-* Levels are to allow expansion of more messages in the future.
-* @param[in] clear If set, will clear the message. Typically this will be set
-* for one reader, so that you don't get the same message over and over.
-* @return bit field to corresponding message.
-*/
-long inv_get_message_level_0(int clear)
-{
- long msg;
- msg = mh.message;
- if (clear) {
- mh.message = 0;
- }
- return msg;
-}
-
-/**
- * @}
- */
diff --git a/libsensors/software/core/mllite/message_layer.h b/libsensors/software/core/mllite/message_layer.h deleted file mode 100644 index df0c0e2..0000000 --- a/libsensors/software/core/mllite/message_layer.h +++ /dev/null @@ -1,35 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_MESSAGE_LAYER_H__
-#define INV_MESSAGE_LAYER_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* Level 0 Type Messages */
- /** A motion event has occured */
-#define INV_MSG_MOTION_EVENT (0x01)
- /** A no motion event has occured */
-#define INV_MSG_NO_MOTION_EVENT (0x02)
- /** A setting of the gyro bias has occured */
-#define INV_MSG_NEW_GB_EVENT (0x04)
- /** A setting of the compass bias has occured */
-#define INV_MSG_NEW_CB_EVENT (0x08)
- /** A setting of the accel bias has occured */
-#define INV_MSG_NEW_AB_EVENT (0x10)
-
- void inv_set_message(long set, long clear, int level);
- long inv_get_message_level_0(int clear);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MESSAGE_LAYER_H__
diff --git a/libsensors/software/core/mllite/ml_math_func.c b/libsensors/software/core/mllite/ml_math_func.c deleted file mode 100644 index 86c4b41..0000000 --- a/libsensors/software/core/mllite/ml_math_func.c +++ /dev/null @@ -1,660 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -/** - * @defgroup ML_MATH_FUNC ml_math_func - * @brief Motion Library - Math Functions - * Common math functions the Motion Library - * - * @{ - * @file ml_math_func.c - * @brief Math Functions. - */ - -#include "mlmath.h" -#include "ml_math_func.h" -#include "mlinclude.h" -#include <string.h> - -/** @internal - * Does the cross product of compass by gravity, then converts that - * to the world frame using the quaternion, then computes the angle that - * is made. - * - * @param[in] compass Compass Vector (Body Frame), length 3 - * @param[in] grav Gravity Vector (Body Frame), length 3 - * @param[in] quat Quaternion, Length 4 - * @return Angle Cross Product makes after quaternion rotation. - */ -float inv_compass_angle(const long *compass, const long *grav, const float *quat) -{ - float cgcross[4], q1[4], q2[4], qi[4]; - float angW; - - // Compass cross Gravity - cgcross[0] = 0.f; - cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; - cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; - cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; - - // Now convert cross product into world frame - inv_q_multf(quat, cgcross, q1); - inv_q_invertf(quat, qi); - inv_q_multf(q1, qi, q2); - - // Protect against atan2 of 0,0 - if ((q2[2] == 0.f) && (q2[1] == 0.f)) - return 0.f; - - // This is the unfiltered heading correction - angW = -atan2f(q2[2], q2[1]); - return angW; -} - -/** - * @brief The gyro data magnitude squared : - * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. - * @param[in] gyro Gyro data scaled with 1 dps = 2^16 - * @return the computed magnitude squared output of the gyroscope. - */ -unsigned long inv_get_gyro_sum_of_sqr(const long *gyro) -{ - unsigned long gmag = 0; - long temp; - int kk; - - for (kk = 0; kk < 3; ++kk) { - temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); - gmag += temp * temp; - } - - return gmag; -} - -/** Performs a multiply and shift by 29. These are good functions to write in assembly on - * with devices with small memory where you want to get rid of the long long which some - * assemblers don't handle well - * @param[in] a - * @param[in] b - * @return ((long long)a*b)>>29 -*/ -long inv_q29_mult(long a, long b) -{ -#ifdef UMPL_ELIMINATE_64BIT - long result; - result = (long)((float)a * b / (1L << 29)); - return result; -#else - long long temp; - long result; - temp = (long long)a * b; - result = (long)(temp >> 29); - return result; -#endif -} - -/** Performs a multiply and shift by 30. These are good functions to write in assembly on - * with devices with small memory where you want to get rid of the long long which some - * assemblers don't handle well - * @param[in] a - * @param[in] b - * @return ((long long)a*b)>>30 -*/ -long inv_q30_mult(long a, long b) -{ -#ifdef UMPL_ELIMINATE_64BIT - long result; - result = (long)((float)a * b / (1L << 30)); - return result; -#else - long long temp; - long result; - temp = (long long)a * b; - result = (long)(temp >> 30); - return result; -#endif -} - -#ifndef UMPL_ELIMINATE_64BIT -long inv_q30_div(long a, long b) -{ - long long temp; - long result; - temp = (((long long)a) << 30) / b; - result = (long)temp; - return result; -} -#endif - -/** Performs a multiply and shift by shift. These are good functions to write - * in assembly on with devices with small memory where you want to get rid of - * the long long which some assemblers don't handle well - * @param[in] a First multicand - * @param[in] b Second multicand - * @param[in] shift Shift amount after multiplying - * @return ((long long)a*b)<<shift -*/ -#ifndef UMPL_ELIMINATE_64BIT -long inv_q_shift_mult(long a, long b, int shift) -{ - long result; - result = (long)(((long long)a * b) >> shift); - return result; -} -#endif - -/** Performs a fixed point quaternion multiply. -* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled -* to 2^30 -* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled -* to 2^30 -* @param[out] qProd Product after quaternion multiply. Length 4. -* 1.0 scaled to 2^30. -*/ -void inv_q_mult(const long *q1, const long *q2, long *qProd) -{ - INVENSENSE_FUNC_START; - qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) - - inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]); - - qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) + - inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]); - - qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) + - inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]); - - qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) - - inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]); -} - -/** Performs a fixed point quaternion addition. -* @param[in] q1 First Quaternion term, length 4. 1.0 scaled -* to 2^30 -* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled -* to 2^30 -* @param[out] qSum Sum after quaternion summation. Length 4. -* 1.0 scaled to 2^30. -*/ -void inv_q_add(long *q1, long *q2, long *qSum) -{ - INVENSENSE_FUNC_START; - qSum[0] = q1[0] + q2[0]; - qSum[1] = q1[1] + q2[1]; - qSum[2] = q1[2] + q2[2]; - qSum[3] = q1[3] + q2[3]; -} - -void inv_vector_normalize(long *vec, int length) -{ - INVENSENSE_FUNC_START; - double normSF = 0; - int ii; - for (ii = 0; ii < length; ii++) { - normSF += - inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]); - } - if (normSF > 0) { - normSF = 1 / sqrt(normSF); - for (ii = 0; ii < length; ii++) { - vec[ii] = (int)((double)vec[ii] * normSF); - } - } else { - vec[0] = 1073741824L; - for (ii = 1; ii < length; ii++) { - vec[ii] = 0; - } - } -} - -void inv_q_normalize(long *q) -{ - INVENSENSE_FUNC_START; - inv_vector_normalize(q, 4); -} - -void inv_q_invert(const long *q, long *qInverted) -{ - INVENSENSE_FUNC_START; - qInverted[0] = q[0]; - qInverted[1] = -q[1]; - qInverted[2] = -q[2]; - qInverted[3] = -q[3]; -} - -double quaternion_to_rotation_angle(const long *quat) { - double quat0 = (double )quat[0] / 1073741824; - if (quat0 > 1.0f) { - quat0 = 1.0; - } else if (quat0 < -1.0f) { - quat0 = -1.0; - } - - return acos(quat0)*2*180/M_PI; -} - -/** Rotates a 3-element vector by Rotation defined by Q -*/ -void inv_q_rotate(const long *q, const long *in, long *out) -{ - long q_temp1[4], q_temp2[4]; - long in4[4], out4[4]; - - // Fixme optimize - in4[0] = 0; - memcpy(&in4[1], in, 3 * sizeof(long)); - inv_q_mult(q, in4, q_temp1); - inv_q_invert(q, q_temp2); - inv_q_mult(q_temp1, q_temp2, out4); - memcpy(out, &out4[1], 3 * sizeof(long)); -} - -void inv_q_multf(const float *q1, const float *q2, float *qProd) -{ - INVENSENSE_FUNC_START; - qProd[0] = - (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); - qProd[1] = - (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); - qProd[2] = - (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); - qProd[3] = - (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); -} - -void inv_q_addf(const float *q1, const float *q2, float *qSum) -{ - INVENSENSE_FUNC_START; - qSum[0] = q1[0] + q2[0]; - qSum[1] = q1[1] + q2[1]; - qSum[2] = q1[2] + q2[2]; - qSum[3] = q1[3] + q2[3]; -} - -void inv_q_normalizef(float *q) -{ - INVENSENSE_FUNC_START; - float normSF = 0; - float xHalf = 0; - normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - if (normSF < 2) { - xHalf = 0.5f * normSF; - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - q[0] *= normSF; - q[1] *= normSF; - q[2] *= normSF; - q[3] *= normSF; - } else { - q[0] = 1.0; - q[1] = 0.0; - q[2] = 0.0; - q[3] = 0.0; - } - normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); -} - -/** Performs a length 4 vector normalization with a square root. -* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero. -*/ -void inv_q_norm4(float *q) -{ - float mag; - mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - if (mag) { - q[0] /= mag; - q[1] /= mag; - q[2] /= mag; - q[3] /= mag; - } else { - q[0] = 1.f; - q[1] = 0.f; - q[2] = 0.f; - q[3] = 0.f; - } -} - -void inv_q_invertf(const float *q, float *qInverted) -{ - INVENSENSE_FUNC_START; - qInverted[0] = q[0]; - qInverted[1] = -q[1]; - qInverted[2] = -q[2]; - qInverted[3] = -q[3]; -} - -/** - * Converts a quaternion to a rotation matrix. - * @param[in] quat 4-element quaternion in fixed point. One is 2^30. - * @param[out] rot Rotation matrix in fixed point. One is 2^30. The - * First 3 elements of the rotation matrix, represent - * the first row of the matrix. Rotation matrix multiplied - * by a 3 element column vector transform a vector from Body - * to World. - */ -void inv_quaternion_to_rotation(const long *quat, long *rot) -{ - rot[0] = - inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], - quat[0]) - - 1073741824L; - rot[1] = - inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); - rot[2] = - inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); - rot[3] = - inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); - rot[4] = - inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], - quat[0]) - - 1073741824L; - rot[5] = - inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); - rot[6] = - inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); - rot[7] = - inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); - rot[8] = - inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], - quat[0]) - - 1073741824L; -} - -/** - * Converts a quaternion to a rotation vector. A rotation vector is - * a method to represent a 4-element quaternion vector in 3-elements. - * To get the quaternion from the 3-elements, The last 3-elements of - * the quaternion will be the given rotation vector. The first element - * of the quaternion will be the positive value that will be required - * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units. - * @param[in] quat 4-element quaternion in fixed point. One is 2^30. - * @param[out] rot Rotation vector in fixed point. One is 2^30. - */ -void inv_quaternion_to_rotation_vector(const long *quat, long *rot) -{ - rot[0] = quat[1]; - rot[1] = quat[2]; - rot[2] = quat[3]; - - if (quat[0] < 0.0) { - rot[0] = -rot[0]; - rot[1] = -rot[1]; - rot[2] = -rot[2]; - } -} - -/** Converts a 32-bit long to a big endian byte stream */ -unsigned char *inv_int32_to_big8(long x, unsigned char *big8) -{ - big8[0] = (unsigned char)((x >> 24) & 0xff); - big8[1] = (unsigned char)((x >> 16) & 0xff); - big8[2] = (unsigned char)((x >> 8) & 0xff); - big8[3] = (unsigned char)(x & 0xff); - return big8; -} - -/** Converts a big endian byte stream into a 32-bit long */ -long inv_big8_to_int32(const unsigned char *big8) -{ - long x; - x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) - | ((long)big8[3]); - return x; -} - -/** Converts a big endian byte stream into a 16-bit integer (short) */ -short inv_big8_to_int16(const unsigned char *big8) -{ - short x; - x = ((short)big8[0] << 8) | ((short)big8[1]); - return x; -} - -/** Converts a little endian byte stream into a 16-bit integer (short) */ -short inv_little8_to_int16(const unsigned char *little8) -{ - short x; - x = ((short)little8[1] << 8) | ((short)little8[0]); - return x; -} - -/** Converts a 16-bit short to a big endian byte stream */ -unsigned char *inv_int16_to_big8(short x, unsigned char *big8) -{ - big8[0] = (unsigned char)((x >> 8) & 0xff); - big8[1] = (unsigned char)(x & 0xff); - return big8; -} - -void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) -{ - int k, l, i, j; - for (i = 0, k = 0; i < *n; i++, k++) { - for (j = 0, l = 0; j < *n; j++, l++) { - if (i == x) - i++; - if (j == y) - j++; - *(b + 6 * k + l) = *(a + 6 * i + j); - } - } - *n = *n - 1; -} - -void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) -{ - int k, l, i, j; - for (i = 0, k = 0; i < *n; i++, k++) { - for (j = 0, l = 0; j < *n; j++, l++) { - if (i == x) - i++; - if (j == y) - j++; - *(b + 6 * k + l) = *(a + 6 * i + j); - } - } - *n = *n - 1; -} - -float inv_matrix_det(float *p, int *n) -{ - float d[6][6], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 7) - *(p + 1) ** (p + 6)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_inc(p, &d[0][0], n, i, j); - sum = - sum + *(p + 6 * i + j) * SIGNM(i + - j) * - inv_matrix_det(&d[0][0], n); - } - - return (sum); -} - -double inv_matrix_detd(double *p, int *n) -{ - double d[6][6], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 7) - *(p + 1) ** (p + 6)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_incd(p, &d[0][0], n, i, j); - sum = - sum + *(p + 6 * i + j) * SIGNM(i + - j) * - inv_matrix_detd(&d[0][0], n); - } - - return (sum); -} - -/** Wraps angle from (-M_PI,M_PI] - * @param[in] ang Angle in radians to wrap - * @return Wrapped angle from (-M_PI,M_PI] - */ -float inv_wrap_angle(float ang) -{ - if (ang > M_PI) - return ang - 2 * (float)M_PI; - else if (ang <= -(float)M_PI) - return ang + 2 * (float)M_PI; - else - return ang; -} - -/** Finds the minimum angle difference ang1-ang2 such that difference - * is between [-M_PI,M_PI] - * @param[in] ang1 - * @param[in] ang2 - * @return angle difference ang1-ang2 - */ -float inv_angle_diff(float ang1, float ang2) -{ - float d; - ang1 = inv_wrap_angle(ang1); - ang2 = inv_wrap_angle(ang2); - d = ang1 - ang2; - if (d > M_PI) - d -= 2 * (float)M_PI; - else if (d < -(float)M_PI) - d += 2 * (float)M_PI; - return d; -} - -/** bernstein hash, derived from public domain source */ -uint32_t inv_checksum(const unsigned char *str, int len) -{ - uint32_t hash = 5381; - int i, c; - - for (i = 0; i < len; i++) { - c = *(str + i); - hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ - } - - return hash; -} - -static unsigned short inv_row_2_scale(const signed char *row) -{ - unsigned short b; - - if (row[0] > 0) - b = 0; - else if (row[0] < 0) - b = 4; - else if (row[1] > 0) - b = 1; - else if (row[1] < 0) - b = 5; - else if (row[2] > 0) - b = 2; - else if (row[2] < 0) - b = 6; - else - b = 7; // error - return b; -} - - -/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. -* @param[in] mtx Orientation matrix to convert to a scalar. -* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the -* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent -* the column the one is on for the second row with bit number 5 being the sign. -* The next 2 bits (6 and 7) represent the column the one is on for the third row with -* bit number 8 being the sign. In binary the identity matrix would therefor be: -* 010_001_000 or 0x88 in hex. -*/ -unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) -{ - - unsigned short scalar; - - /* - XYZ 010_001_000 Identity Matrix - XZY 001_010_000 - YXZ 010_000_001 - YZX 000_010_001 - ZXY 001_000_010 - ZYX 000_001_010 - */ - - scalar = inv_row_2_scale(mtx); - scalar |= inv_row_2_scale(mtx + 3) << 3; - scalar |= inv_row_2_scale(mtx + 6) << 6; - - - return scalar; -} - -/** Uses the scalar orientation value to convert from chip frame to body frame -* @param[in] orientation A scalar that represent how to go from chip to body frame -* @param[in] input Input vector, length 3 -* @param[out] output Output vector, length 3 -*/ -void inv_convert_to_body(unsigned short orientation, const long *input, long *output) -{ - output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004); - output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020); - output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100); -} - -/** Uses the scalar orientation value to convert from body frame to chip frame -* @param[in] orientation A scalar that represent how to go from chip to body frame -* @param[in] input Input vector, length 3 -* @param[out] output Output vector, length 3 -*/ -void inv_convert_to_chip(unsigned short orientation, const long *input, long *output) -{ - output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004); - output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020); - output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100); -} - - -/** Uses the scalar orientation value to convert from chip frame to body frame and -* apply appropriate scaling. -* @param[in] orientation A scalar that represent how to go from chip to body frame -* @param[in] sensitivity Sensitivity scale -* @param[in] input Input vector, length 3 -* @param[out] output Output vector, length 3 -*/ -void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output) -{ - output[0] = inv_q30_mult(input[orientation & 0x03] * - SIGNSET(orientation & 0x004), sensitivity); - output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] * - SIGNSET(orientation & 0x020), sensitivity); - output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] * - SIGNSET(orientation & 0x100), sensitivity); -} - -/** find a norm for a vector -* @param[in] a vector [3x1] -* @param[out] output the norm of the input vector -*/ -double inv_vector_norm(const float *x) -{ - return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); -} -/** - * @} - */ diff --git a/libsensors/software/core/mllite/ml_math_func.h b/libsensors/software/core/mllite/ml_math_func.h deleted file mode 100644 index 916de0a..0000000 --- a/libsensors/software/core/mllite/ml_math_func.h +++ /dev/null @@ -1,108 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#ifndef INVENSENSE_INV_MATH_FUNC_H__ -#define INVENSENSE_INV_MATH_FUNC_H__ - -#include "mltypes.h" - -#define GYRO_MAG_SQR_SHIFT 6 -#define NUM_ROTATION_MATRIX_ELEMENTS (9) -#define ROT_MATRIX_SCALE_LONG (1073741824L) -#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) -#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ - ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) -#define SIGNM(k)((int)(k)&1?-1:1) -#define SIGNSET(x) ((x) ? -1 : +1) - -#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f - -#ifdef __cplusplus -extern "C" { -#endif - - static inline float inv_q30_to_float(long q30) - { - return (float) q30 / ((float)(1L << 30)); - } - - static inline double inv_q30_to_double(long q30) - { - return (double) q30 / ((double)(1L << 30)); - } - - static inline float inv_q16_to_float(long q16) - { - return (float) q16 / (1L << 16); - } - - static inline double inv_q16_to_double(long q16) - { - return (double) q16 / (1L << 16); - } - - - - - long inv_q29_mult(long a, long b); - long inv_q30_mult(long a, long b); - - /* UMPL_ELIMINATE_64BIT Notes: - * An alternate implementation using float instead of long long accudoublemulators - * is provided for q29_mult and q30_mult. - * When long long accumulators are used and an alternate implementation is not - * available, we eliminate the entire function and header with a macro. - */ -#ifndef UMPL_ELIMINATE_64BIT - long inv_q30_div(long a, long b); - long inv_q_shift_mult(long a, long b, int shift); -#endif - - void inv_q_mult(const long *q1, const long *q2, long *qProd); - void inv_q_add(long *q1, long *q2, long *qSum); - void inv_q_normalize(long *q); - void inv_q_invert(const long *q, long *qInverted); - void inv_q_multf(const float *q1, const float *q2, float *qProd); - void inv_q_addf(const float *q1, const float *q2, float *qSum); - void inv_q_normalizef(float *q); - void inv_q_norm4(float *q); - void inv_q_invertf(const float *q, float *qInverted); - void inv_quaternion_to_rotation(const long *quat, long *rot); - unsigned char *inv_int32_to_big8(long x, unsigned char *big8); - long inv_big8_to_int32(const unsigned char *big8); - short inv_big8_to_int16(const unsigned char *big8); - short inv_little8_to_int16(const unsigned char *little8); - unsigned char *inv_int16_to_big8(short x, unsigned char *big8); - float inv_matrix_det(float *p, int *n); - void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); - double inv_matrix_detd(double *p, int *n); - void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); - float inv_wrap_angle(float ang); - float inv_angle_diff(float ang1, float ang2); - void inv_quaternion_to_rotation_vector(const long *quat, long *rot); - unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); - void inv_convert_to_body(unsigned short orientation, const long *input, long *output); - void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); - void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); - void inv_q_rotate(const long *q, const long *in, long *out); - void inv_vector_normalize(long *vec, int length); - uint32_t inv_checksum(const unsigned char *str, int len); - float inv_compass_angle(const long *compass, const long *grav, - const float *quat); - unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); - - static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) - { - return (long)((t1 - t2) / 1000000L); - } - - double quaternion_to_rotation_angle(const long *quat); - double inv_vector_norm(const float *x); - -#ifdef __cplusplus -} -#endif -#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/libsensors/software/core/mllite/mlmath.c b/libsensors/software/core/mllite/mlmath.c deleted file mode 100644 index 5eb4264..0000000 --- a/libsensors/software/core/mllite/mlmath.c +++ /dev/null @@ -1,68 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/******************************************************************************* - * - * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#include <math.h> - -double ml_asin(double x) -{ - return asin(x); -} - -double ml_atan(double x) -{ - return atan(x); -} - -double ml_atan2(double x, double y) -{ - return atan2(x, y); -} - -double ml_log(double x) -{ - return log(x); -} - -double ml_sqrt(double x) -{ - return sqrt(x); -} - -double ml_ceil(double x) -{ - return ceil(x); -} - -double ml_floor(double x) -{ - return floor(x); -} - -double ml_cos(double x) -{ - return cos(x); -} - -double ml_sin(double x) -{ - return sin(x); -} - -double ml_acos(double x) -{ - return acos(x); -} - -double ml_pow(double x, double y) -{ - return pow(x, y); -} diff --git a/libsensors/software/core/mllite/mpl.c b/libsensors/software/core/mllite/mpl.c deleted file mode 100644 index 231cbfd..0000000 --- a/libsensors/software/core/mllite/mpl.c +++ /dev/null @@ -1,72 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup MPL mpl
- * @brief Motion Library - Start Point
- * Initializes MPL.
- *
- * @{
- * @file mpl.c
- * @brief MPL start point.
- */
-
-#include "storage_manager.h"
-#include "log.h"
-#include "mpl.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "results_holder.h"
-#include "mlinclude.h"
-
-/**
- * @brief Initializes the MPL. Should be called first and once
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_init_mpl(void)
-{
- inv_init_storage_manager();
-
- /* initialize the start callback manager */
- INV_ERROR_CHECK(inv_init_start_manager());
-
- /* initialize the data builder */
- INV_ERROR_CHECK(inv_init_data_builder());
-
- INV_ERROR_CHECK(inv_enable_results_holder());
-
- return INV_SUCCESS;
-}
-
-const char ml_ver[] = "InvenSense MPL 5.0.1";
-
-/**
- * @brief used to get the MPL version.
- * @param version a string where the MPL version gets stored.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_get_version(char **version)
-{
- INVENSENSE_FUNC_START;
- /* cast out the const */
- *version = (char *)&ml_ver;
- return INV_SUCCESS;
-}
-
-/**
- * @brief Starts the MPL. Typically called after inv_init_mpl() or after a
- * inv_stop_mpl() to start the MPL back up an running.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_start_mpl(void)
-{
- INV_ERROR_CHECK(inv_execute_mpl_start_notification());
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/libsensors/software/core/mllite/mpl.h b/libsensors/software/core/mllite/mpl.h deleted file mode 100644 index a6b5ac7..0000000 --- a/libsensors/software/core/mllite/mpl.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_MPL_H__ -#define INV_MPL_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_init_mpl(void); -inv_error_t inv_start_mpl(void); -inv_error_t inv_get_version(char **version); - -#ifdef __cplusplus -} -#endif - -#endif // INV_MPL_H__ diff --git a/libsensors/software/core/mllite/results_holder.c b/libsensors/software/core/mllite/results_holder.c deleted file mode 100644 index 97ffdec..0000000 --- a/libsensors/software/core/mllite/results_holder.c +++ /dev/null @@ -1,500 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @defgroup Results_Holder results_holder - * @brief Motion Library - Results Holder - * Holds the data for MPL - * - * @{ - * @file results_holder.c - * @brief Results Holder for HAL. - */ -#include "results_holder.h" -#include "log.h" -#include "ml_math_func.h" -#include "mlmath.h" -#include "start_manager.h" -#include "data_builder.h" -#include "message_layer.h" - -// These 2 status bits are used to control when the 9 axis quaternion is updated -#define INV_COMPASS_CORRECTION_SET 1 -#define INV_6_AXIS_QUAT_SET 2 - -struct results_t { - long nav_quat[4]; - long gam_quat[4]; - inv_time_t nav_timestamp; - inv_time_t gam_timestamp; - long local_field[3]; /**< local earth's magnetic field */ - long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ - long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ - int acc_state; /**< Describes accel state */ - long compass_bias_error[3]; /**< Error Squared */ - unsigned char motion_state; - unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ - long compass_count; /**< compass state internal counter */ - int got_compass_bias; /**< Flag describing if compass bias is known */ - int large_mag_field; /**< Flag describing if there is a large magnetic field */ - int compass_state; /**< Internal compass state */ - long status; - struct inv_sensor_cal_t *sensor; - float quat_confidence_interval; -}; -static struct results_t rh; - -/** @internal -* Store a quaternion more suitable for gaming. This quaternion is often determined -* using only gyro and accel. -* @param[in] quat Length 4, Quaternion scaled by 2^30 -*/ -void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) -{ - rh.status |= INV_6_AXIS_QUAT_SET; - memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); - rh.gam_timestamp = timestamp; -} - -/** @internal -* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. -* @param[in] data Quaternion Adjustment -* @param[in] timestamp Timestamp of when this is valid -*/ -void inv_set_compass_correction(const long *data, inv_time_t timestamp) -{ - rh.status |= INV_COMPASS_CORRECTION_SET; - memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); - rh.nav_timestamp = timestamp; -} - -/** @internal -* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. -* @param[out] data Quaternion Adjustment -* @param[out] timestamp Timestamp of when this is valid -*/ -void inv_get_compass_correction(long *data, inv_time_t *timestamp) -{ - memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); - *timestamp = rh.nav_timestamp; -} - -/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. - * @return Returns non-zero if there is a large magnetic field. - */ -int inv_get_large_mag_field() -{ - return rh.large_mag_field; -} - -/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. - * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. - */ -void inv_set_large_mag_field(int state) -{ - rh.large_mag_field = state; -} - -/** Gets the accel state set by inv_set_acc_state() - * @return accel state. - */ -int inv_get_acc_state() -{ - return rh.acc_state; -} - -/** Sets the accel state. See inv_get_acc_state() to get the value. - * @param[in] state value to set accel state to. - */ -void inv_set_acc_state(int state) -{ - rh.acc_state = state; - return; -} - -/** Returns the motion state -* @param[out] cntr Number of previous times a no motion event has occured in a row. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -int inv_get_motion_state(unsigned int *cntr) -{ - *cntr = rh.motion_state_counter; - return rh.motion_state; -} - -/** Sets the motion state - * @param[in] state motion state where INV_NO_MOTION is not moving - * and INV_MOTION is moving. - */ -void inv_set_motion_state(unsigned char state) -{ - long set; - if (state == rh.motion_state) { - if (state == INV_NO_MOTION) { - rh.motion_state_counter++; - } else { - rh.motion_state_counter = 0; - } - return; - } - rh.motion_state_counter = 0; - rh.motion_state = state; - /* Equivalent to set = state, but #define's may change. */ - if (state == INV_MOTION) - set = INV_MSG_MOTION_EVENT; - else - set = INV_MSG_NO_MOTION_EVENT; - inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); -} - -/** Sets the local earth's magnetic field -* @param[in] data Local earth's magnetic field in uT scaled by 2^16. -* Length = 3. Y typically points north, Z typically points down in -* northern hemisphere and up in southern hemisphere. -*/ -void inv_set_local_field(const long *data) -{ - memcpy(rh.local_field, data, sizeof(rh.local_field)); -} - -/** Gets the local earth's magnetic field -* @param[out] data Local earth's magnetic field in uT scaled by 2^16. -* Length = 3. Y typically points north, Z typically points down in -* northern hemisphere and up in southern hemisphere. -*/ -void inv_get_local_field(long *data) -{ - memcpy(data, rh.local_field, sizeof(rh.local_field)); -} - -/** Sets the compass sensitivity - * @param[in] data Length 3, sensitivity for each compass axis - * scaled such that 1.0 = 2^30. - */ -void inv_set_mag_scale(const long *data) -{ - memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); -} - -/** Gets the compass sensitivity - * @param[out] data Length 3, sensitivity for each compass axis - * scaled such that 1.0 = 2^30. - */ -void inv_get_mag_scale(long *data) -{ - memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); -} - -/** Gets gravity vector - * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_gravity(long *data) -{ - data[0] = - inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); - data[1] = - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); - data[2] = - (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) - - 1073741824L; - - return INV_SUCCESS; -} - -/** Returns a quaternion based only on gyro and accel. - * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_6axis_quaternion(long *data) -{ - memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); - return INV_SUCCESS; -} - -/** Returns a quaternion. - * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_quaternion(long *data) -{ - if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { - inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); - rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); - } - memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); - return INV_SUCCESS; -} - -/** Returns a quaternion. - * @param[out] data 9-axis quaternion. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_quaternion_float(float *data) -{ - long ldata[4]; - inv_error_t result = inv_get_quaternion(ldata); - data[0] = inv_q30_to_float(ldata[0]); - data[1] = inv_q30_to_float(ldata[1]); - data[2] = inv_q30_to_float(ldata[2]); - data[3] = inv_q30_to_float(ldata[3]); - return result; -} - -/** Returns a quaternion with accuracy and timestamp. - * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. - * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. - * @param[out] timestamp Timestamp of this quaternion in nanoseconds - */ -void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) -{ - inv_get_quaternion(data); - *timestamp = inv_get_last_timestamp(); - if (inv_get_compass_on()) { - *accuracy = inv_get_mag_accuracy(); - } else if (inv_get_gyro_on()) { - *accuracy = inv_get_gyro_accuracy(); - }else if (inv_get_accel_on()) { - *accuracy = inv_get_accel_accuracy(); - } else { - *accuracy = 0; - } -} - -/** Callback that gets called everytime there is new data. It is - * registered by inv_start_results_holder(). - * @param[in] sensor_cal New sensor data to process. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) -{ - rh.sensor = sensor_cal; - return INV_SUCCESS; -} - -/** Function to turn on this module. This is automatically called by - * inv_enable_results_holder(). Typically not called by users. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_start_results_holder(void) -{ - inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, - INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); - return INV_SUCCESS; -} - -/** Initializes results holder. This is called automatically by the -* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but -* is typically not needed to be called by outside callers. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_init_results_holder(void) -{ - memset(&rh, 0, sizeof(rh)); - rh.mag_scale[0] = 1L<<30; - rh.mag_scale[1] = 1L<<30; - rh.mag_scale[2] = 1L<<30; - rh.compass_correction[0] = 1L<<30; - rh.gam_quat[0] = 1L<<30; - rh.nav_quat[0] = 1L<<30; - rh.quat_confidence_interval = (float)M_PI; - return INV_SUCCESS; -} - -/** Turns on storage of results. -*/ -inv_error_t inv_enable_results_holder() -{ - inv_error_t result; - result = inv_init_results_holder(); - if ( result ) { - return result; - } - - result = inv_register_mpl_start_notification(inv_start_results_holder); - return result; -} - -/** Sets state of if we know the compass bias. - * @return return 1 if we know the compass bias, 0 if not. - * it is set with inv_set_compass_bias_found() - */ -int inv_got_compass_bias() -{ - return rh.got_compass_bias; -} - -/** Sets whether we know the compass bias - * @param[in] state Set to 1 if we know the compass bias. - * Can be retrieved with inv_got_compass_bias() - */ -void inv_set_compass_bias_found(int state) -{ - rh.got_compass_bias = state; -} - -/** Sets the compass state. - * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). - */ -void inv_set_compass_state(int state) -{ - rh.compass_state = state; -} - -/** Get's the compass state - * @return the compass state that was set with inv_set_compass_state() - */ -int inv_get_compass_state() -{ - return rh.compass_state; -} - -/** Set compass bias error. See inv_get_compass_bias_error() - * @param[in] bias_error Set's how accurate we know the compass bias. It is the - * error squared. - */ -void inv_set_compass_bias_error(const long *bias_error) -{ - memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); -} - -/** Get's compass bias error. See inv_set_compass_bias_error() for setting. - * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. - */ -void inv_get_compass_bias_error(long *bias_error) -{ - memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * with gravity removed - * @param[out] data 3-element vector of accelerometer data in body frame - * with gravity removed - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_linear_accel(long *data) -{ - long gravity[3]; - - if (data != NULL) - { - inv_get_accel_set(data, NULL, NULL); - inv_get_gravity(gravity); - data[0] -= gravity[0] >> 14; - data[1] -= gravity[1] >> 14; - data[2] -= gravity[2] >> 14; - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * @param[out] data 3-element vector of accelerometer data in body frame - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_accel(long *data) -{ - if (data != NULL) { - inv_get_accel_set(data, NULL, NULL); - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of accelerometer float data - * @param[out] data 3-element vector of accelerometer float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_accel_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL && !inv_get_accel(tdata)) { - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of gyro float data - * @param[out] data 3-element vector of gyro float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_gyro_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL) { - inv_get_gyro_set(tdata, NULL, NULL); - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** Set 9 axis 95% heading confidence interval for quaternion -* @param[in] ci Confidence interval in radians. -*/ -void inv_set_heading_confidence_interval(float ci) -{ - rh.quat_confidence_interval = ci; -} - -/** Get 9 axis 95% heading confidence interval for quaternion -* @return Confidence interval in radians. -*/ -float inv_get_heading_confidence_interval(void) -{ - return rh.quat_confidence_interval; -} - -/** - * @brief Returns 3-element vector of linear accel float data - * @param[out] data 3-element vector of linear aceel float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_linear_accel_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL && !inv_get_linear_accel(tdata)) { - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @} - */ diff --git a/libsensors/software/core/mllite/results_holder.h b/libsensors/software/core/mllite/results_holder.h deleted file mode 100644 index a60d7f0..0000000 --- a/libsensors/software/core/mllite/results_holder.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_RESULTS_HOLDER_H__ -#define INV_RESULTS_HOLDER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#define INV_MOTION 0x0001 -#define INV_NO_MOTION 0x0002 - - /**************************************************************************/ - /* The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = */ - /* 2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even. */ - /* The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = */ - /* 2^ACC_MAG_SQR_SHIFT */ - /**************************************************************************/ -#define ACC_MAG_SQR_SHIFT 16 - -void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp); - -// States -#define SF_NORMAL 0 -#define SF_UNCALIBRATED 1 -#define SF_STARTUP_SETTLE 2 -#define SF_FAST_SETTLE 3 -#define SF_DISTURBANCE 4 -#define SF_SLOW_SETTLE 5 - -int inv_get_acc_state(); -void inv_set_acc_state(int state); -int inv_get_motion_state(unsigned int *cntr); -void inv_set_motion_state(unsigned char state); -inv_error_t inv_get_gravity(long *data); -inv_error_t inv_get_6axis_quaternion(long *data); -inv_error_t inv_get_quaternion(long *data); -inv_error_t inv_get_quaternion_float(float *data); -void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp); - -inv_error_t inv_enable_results_holder(); -inv_error_t inv_init_results_holder(void); - -/* Magnetic Field Parameters*/ -void inv_set_local_field(const long *data); -void inv_get_local_field(long *data); -void inv_set_mag_scale(const long *data); -void inv_get_mag_scale(long *data); -void inv_set_compass_correction(const long *data, inv_time_t timestamp); -void inv_get_compass_correction(long *data, inv_time_t *timestamp); -int inv_got_compass_bias(); -void inv_set_compass_bias_found(int state); -int inv_get_large_mag_field(); -void inv_set_large_mag_field(int state); -void inv_set_compass_state(int state); -int inv_get_compass_state(); -void inv_set_compass_bias_error(const long *bias_error); -void inv_get_compass_bias_error(long *bias_error); -inv_error_t inv_get_linear_accel(long *data); -inv_error_t inv_get_accel(long *data); -inv_error_t inv_get_accel_float(float *data); -inv_error_t inv_get_gyro_float(float *data); -inv_error_t inv_get_linear_accel_float(float *data); -void inv_set_heading_confidence_interval(float ci); -float inv_get_heading_confidence_interval(void); - -#ifdef __cplusplus -} -#endif - -#endif // INV_RESULTS_HOLDER_H__ diff --git a/libsensors/software/core/mllite/start_manager.c b/libsensors/software/core/mllite/start_manager.c deleted file mode 100644 index fb758e7..0000000 --- a/libsensors/software/core/mllite/start_manager.c +++ /dev/null @@ -1,105 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-/**
- * @defgroup Start_Manager start_manager
- * @brief Motion Library - Start Manager
- * Start Manager
- *
- * @{
- * @file start_manager.c
- * @brief This handles all the callbacks when inv_start_mpl() is called.
- */
-
-
-#include <string.h>
-#include "log.h"
-#include "start_manager.h"
-
-typedef inv_error_t (*inv_start_cb_func)();
-struct inv_start_cb_t {
- int num_cb;
- inv_start_cb_func start_cb[INV_MAX_START_CB];
-};
-
-static struct inv_start_cb_t inv_start_cb;
-
-/** Initilize the start manager. Typically called by inv_start_mpl();
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_start_manager(void)
-{
- memset(&inv_start_cb, 0, sizeof(inv_start_cb));
- return INV_SUCCESS;
-}
-
-/** Removes a callback from start notification
-* @param[in] start_cb function to remove from start notification
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- int kk;
-
- for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
- if (inv_start_cb.start_cb[kk] == start_cb) {
- // Found the match
- if (kk != (inv_start_cb.num_cb-1)) {
- memmove(&inv_start_cb.start_cb[kk],
- &inv_start_cb.start_cb[kk+1],
- (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
- }
- inv_start_cb.num_cb--;
- return INV_SUCCESS;
- }
- }
- return INV_ERROR_INVALID_PARAMETER;
-}
-
-/** Register a callback to receive when inv_start_mpl() is called.
-* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- if (inv_start_cb.num_cb >= INV_MAX_START_CB)
- return INV_ERROR_INVALID_PARAMETER;
-
- inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
- inv_start_cb.num_cb++;
- return INV_SUCCESS;
-}
-
-/** Callback all the functions that want to be notified when inv_start_mpl() was
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_execute_mpl_start_notification(void)
-{
- inv_error_t result,first_error;
- int kk;
-
- first_error = INV_SUCCESS;
-
- for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
- result = inv_start_cb.start_cb[kk]();
- if (result && (first_error == INV_SUCCESS)) {
- first_error = result;
- }
- }
- return first_error;
-}
-
-/**
- * @}
- */
diff --git a/libsensors/software/core/mllite/start_manager.h b/libsensors/software/core/mllite/start_manager.h deleted file mode 100644 index 899e3f5..0000000 --- a/libsensors/software/core/mllite/start_manager.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#ifndef INV_START_MANAGER_H__ -#define INV_START_MANAGER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" - -/** Max number of start callbacks we can handle. */ -#define INV_MAX_START_CB 20 - -inv_error_t inv_init_start_manager(void); -inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void)); -inv_error_t inv_execute_mpl_start_notification(void); -inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void)); - -#ifdef __cplusplus -} -#endif -#endif // INV_START_MANAGER_H__ diff --git a/libsensors/software/core/mllite/storage_manager.c b/libsensors/software/core/mllite/storage_manager.c deleted file mode 100644 index 4b92bfc..0000000 --- a/libsensors/software/core/mllite/storage_manager.c +++ /dev/null @@ -1,200 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @defgroup Storage_Manager storage_manager - * @brief Motion Library - Stores Data for functions. - * - * - * @{ - * @file storage_manager.c - * @brief Load and Store Manager. - */ -#include "storage_manager.h" -#include "log.h" -#include "ml_math_func.h" -#include "mlmath.h" - -/* Must be changed if the format of storage changes */ -#define DEFAULT_KEY 29681 - -typedef inv_error_t (*load_func_t)(const unsigned char *data); -typedef inv_error_t (*save_func_t)(unsigned char *data); -/** Max number of entites that can be stored */ -#define NUM_STORAGE_BOXES 20 - -struct data_header_t { - long size; - uint32_t checksum; - unsigned int key; -}; - -struct data_storage_t { - int num; /**< Number of differnt save entities */ - size_t total_size; /**< Size in bytes to store non volatile data */ - load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ - save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ - struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ -}; -static struct data_storage_t ds; - -/** Should be called once before using any of the storage methods. Typically -* called first by inv_init_mpl().*/ -void inv_init_storage_manager() -{ - memset(&ds, 0, sizeof(ds)); - ds.total_size = sizeof(struct data_header_t); -} - -/** Used to register your mechanism to load and store non-volative data. This should typical be -* called during the enable function for your feature. -* @param[in] load_func function pointer you will use to receive data that was stored for you. -* @param[in] save_func function pointer you will use to save any data you want saved to -* non-volatile memory between runs. -* @param[in] size The size in bytes of the amount of data you want loaded and saved. -* @param[in] key The key associated with your data type should be unique across MPL. -* The key should change when your type of data for storage changes. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), - inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) -{ - int kk; - // Check if this has been registered already - for (kk=0; kk<ds.num; ++kk) { - if (key == ds.hd[kk].key) { - return INV_ERROR_INVALID_PARAMETER; - } - } - // Make sure there is room - if (ds.num >= NUM_STORAGE_BOXES) { - return INV_ERROR_INVALID_PARAMETER; - } - // Add to list - ds.hd[ds.num].key = key; - ds.hd[ds.num].size = size; - ds.load[ds.num] = load_func; - ds.save[ds.num] = save_func; - ds.total_size += size + sizeof(struct data_header_t); - ds.num++; - - return INV_SUCCESS; -} - -/** Returns the memory size needed to perform a store -* @param[out] size Size in bytes of memory needed to store. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_get_mpl_state_size(size_t *size) -{ - *size = ds.total_size; - return INV_SUCCESS; -} - -/** @internal - * Finds key in ds.hd[] array and returns location - * @return location where key exists in array, -1 if not found. - */ -static int inv_find_entry(unsigned int key) -{ - int kk; - for (kk=0; kk<ds.num; ++kk) { - if (key == ds.hd[kk].key) { - return kk; - } - } - return -1; -} - -/** This function takes a block of data that has been saved in non-volatile memory and pushes -* to the proper locations. Multiple error checks are performed on the data. -* @param[in] data Data that was saved to be loaded up by MPL -* @param[in] length Length of data vector in bytes -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) -{ - struct data_header_t *hd; - int entry; - uint32_t checksum; - long len; - - len = length; // Important so we get negative numbers - if (len < sizeof(struct data_header_t)) - return INV_ERROR_CALIBRATION_LOAD; // No data - hd = (struct data_header_t *)data; - if (hd->key != DEFAULT_KEY) - return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption - len = MIN(hd->size, len); - len = hd->size; - len -= sizeof(struct data_header_t); - data += sizeof(struct data_header_t); - checksum = inv_checksum(data, len); - if (checksum != hd->checksum) - return INV_ERROR_CALIBRATION_LOAD; // Data corruption - - while (len > (long)sizeof(struct data_header_t)) { - hd = (struct data_header_t *)data; - entry = inv_find_entry(hd->key); - data += sizeof(struct data_header_t); - len -= sizeof(struct data_header_t); - if (entry >= 0 && len >= hd->size) { - if (hd->size == ds.hd[entry].size) { - checksum = inv_checksum(data, hd->size); - if (checksum == hd->checksum) { - ds.load[entry](data); - } else { - return INV_ERROR_CALIBRATION_LOAD; - } - } - } - len -= hd->size; - if (len >= 0) - data = data + hd->size; - } - - return INV_SUCCESS; -} - -/** This function fills up a block of memory to be stored in non-volatile memory. -* @param[out] data Place to store data, size of sz, must be at least size -* returned by inv_get_mpl_state_size() -* @param[in] sz Size of data. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) -{ - unsigned char *cur; - int kk; - struct data_header_t *hd; - - if (sz >= ds.total_size) { - cur = data + sizeof(struct data_header_t); - for (kk = 0; kk < ds.num; ++kk) { - hd = (struct data_header_t *)cur; - cur += sizeof(struct data_header_t); - ds.save[kk](cur); - hd->checksum = inv_checksum(cur, ds.hd[kk].size); - hd->size = ds.hd[kk].size; - hd->key = ds.hd[kk].key; - cur += ds.hd[kk].size; - } - } else { - return INV_ERROR_CALIBRATION_LOAD; - } - - hd = (struct data_header_t *)data; - hd->checksum = inv_checksum(data + sizeof(struct data_header_t), - ds.total_size - sizeof(struct data_header_t)); - hd->key = DEFAULT_KEY; - hd->size = ds.total_size; - - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/libsensors/software/core/mllite/storage_manager.h b/libsensors/software/core/mllite/storage_manager.h deleted file mode 100644 index 7255591..0000000 --- a/libsensors/software/core/mllite/storage_manager.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - $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__ */ |