diff options
| author | Kyle Repinski <repinski23@gmail.com> | 2015-12-08 03:42:06 -0600 |
|---|---|---|
| committer | Ziyan <jaraidaniel@gmail.com> | 2016-01-17 22:41:00 +0100 |
| commit | 279fa1241dec2d666d92eec566a6cabfc1ad9f86 (patch) | |
| tree | e8160418ed6b283386074a816d969916f497ba21 | |
| parent | 3f2cb86a83bd0bc394399b7cd52f3db5339dc2d9 (diff) | |
| download | device_samsung_tuna-279fa1241dec2d666d92eec566a6cabfc1ad9f86.zip device_samsung_tuna-279fa1241dec2d666d92eec566a6cabfc1ad9f86.tar.gz device_samsung_tuna-279fa1241dec2d666d92eec566a6cabfc1ad9f86.tar.bz2 | |
libsensors: mlsdk: Remove lots of unused cruft.
This alone was enough to cut 16KB off of the mllite.so size.
37 files changed, 433 insertions, 4046 deletions
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp index ed6631c..481676a 100644 --- a/libsensors/MPLSensor.cpp +++ b/libsensors/MPLSensor.cpp @@ -84,6 +84,7 @@ void mot_cb_wrapper(uint16_t val) if (gMPLSensor) { gMPLSensor->cbOnMotion(val); } + ALOGI("%s: Got motion callback! Value: %hu", __func__, val); } void procData_cb_wrapper() diff --git a/libsensors/mlsdk/Android.mk b/libsensors/mlsdk/Android.mk index d8c0724..cb97a75 100644 --- a/libsensors/mlsdk/Android.mk +++ b/libsensors/mlsdk/Android.mk @@ -39,24 +39,19 @@ ifeq ($(BOARD_INVENSENSE_APPLY_COMPASS_NOISE_FILTER),true) endif LOCAL_C_INCLUDES := \ - $(MLSDK_PATH)/mldmp \ $(MLSDK_PATH)/mllite \ + $(MLSDK_PATH)/mlutils \ $(MLSDK_PATH)/platform/include \ $(MLSDK_PATH)/platform/include/linux \ - $(MLSDK_PATH)/mlutils \ - $(MLSDK_PATH)/mlapps/common \ - $(MLSDK_PATH)/mllite/akmd \ $(MLSDK_PATH)/platform/linux LOCAL_SRC_FILES := \ mlsdk/mllite/accel.c \ mlsdk/mllite/compass.c \ - mlsdk/mllite/pressure.c \ mlsdk/mllite/mldl_cfg_mpu.c \ mlsdk/mllite/dmpDefault.c \ mlsdk/mllite/ml.c \ mlsdk/mllite/mlarray.c \ - mlsdk/mllite/mlarray_legacy.c \ mlsdk/mllite/mlFIFO.c \ mlsdk/mllite/mlFIFOHW.c \ mlsdk/mllite/mlMathFunc.c \ @@ -68,9 +63,8 @@ LOCAL_SRC_FILES := \ mlsdk/mllite/mlsupervisor.c \ mlsdk/mllite/mlBiasNoMotion.c \ mlsdk/mllite/mlSetGyroBias.c \ - mlsdk/mllite/ml_mputest.c \ - mlsdk/mlutils/mputest.c \ - mlsdk/mlutils/checksum.c + mlsdk/mllite/mlcompat.c \ + mlsdk/mlutils/checksum.c \ LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform include $(BUILD_SHARED_LIBRARY) diff --git a/libsensors/mlsdk/mllite/accel.c b/libsensors/mlsdk/mllite/accel.c index b8a0ed2..c235a27 100644 --- a/libsensors/mlsdk/mllite/accel.c +++ b/libsensors/mlsdk/mllite/accel.c @@ -93,34 +93,6 @@ unsigned char inv_accel_present(void) } /** - * @brief Query the accel slave address. - * @return The 7-bit accel slave address. - */ -unsigned char inv_get_slave_addr(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pdata) - return mldl_cfg->pdata->accel.address; - else - return 0; -} - -/** - * @brief Get the ID of the accel in use. - * @return ID of the accel in use. - */ -unsigned short inv_get_accel_id(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->accel) { - return mldl_cfg->accel->id; - } - return ID_INVALID; -} - -/** * @brief Get a sample of accel data from the device. * @param data * the buffer to store the accel raw data for diff --git a/libsensors/mlsdk/mllite/accel.h b/libsensors/mlsdk/mllite/accel.h index d3fbc6a..2190233 100644 --- a/libsensors/mlsdk/mllite/accel.h +++ b/libsensors/mlsdk/mllite/accel.h @@ -30,9 +30,6 @@ extern "C" { #include "mltypes.h" #include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "accel_legacy.h" -#endif /* ------------ */ /* - Defines. - */ @@ -47,9 +44,7 @@ extern "C" { /* --------------------- */ unsigned char inv_accel_present(void); - unsigned char inv_get_slave_addr(void); inv_error_t inv_get_accel_data(long *data); - unsigned short inv_get_accel_id(void); #ifdef __cplusplus } diff --git a/libsensors/mlsdk/mllite/compass.c b/libsensors/mlsdk/mllite/compass.c index c008fbf..c8381de 100644 --- a/libsensors/mlsdk/mllite/compass.c +++ b/libsensors/mlsdk/mllite/compass.c @@ -227,34 +227,6 @@ unsigned char inv_compass_present(void) } /** - * @brief Query the compass slave address. - * @return The 7-bit compass slave address. - */ -unsigned char inv_get_compass_slave_addr(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pdata) - return mldl_cfg->pdata->compass.address; - else - return 0; -} - -/** - * @brief Get the ID of the compass in use. - * @return ID of the compass in use. - */ -unsigned short inv_get_compass_id(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->compass) { - return mldl_cfg->compass->id; - } - return ID_INVALID; -} - -/** * @brief Get a sample of compass data from the device. * @param data * the buffer to store the compass raw data for @@ -365,73 +337,6 @@ inv_error_t inv_set_compass_bias(long *bias) } /** - * @brief Write a single register on the compass slave device, regardless - * of the bus it is connected to and the MPU's configuration. - * @param reg - * the register to write to on the slave compass device. - * @param val - * the value to write. - * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. - */ -inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val) -{ - struct ext_slave_config config; - unsigned char data[2]; - inv_error_t result; - - data[0] = reg; - data[1] = val; - - config.key = MPU_SLAVE_WRITE_REGISTERS; - config.len = 2; - config.apply = TRUE; - config.data = data; - - result = inv_mpu_config_compass(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief Read values from the compass slave device registers, regardless - * of the bus it is connected to and the MPU's configuration. - * @param reg - * the register to read from on the slave compass device. - * @param val - * a buffer of 3 elements to store the values read from the - * compass device. - * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. - */ -inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val) -{ - struct ext_slave_config config; - unsigned char data[2]; - inv_error_t result; - - data[0] = reg; - - config.key = MPU_SLAVE_READ_REGISTERS; - config.len = 2; - config.apply = TRUE; - config.data = data; - - result = inv_mpu_get_compass_config(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - *val = data[1]; - return result; -} - -/** * @brief Read values from the compass slave device scale registers, regardless * of the bus it is connected to and the MPU's configuration. * @param reg diff --git a/libsensors/mlsdk/mllite/compass.h b/libsensors/mlsdk/mllite/compass.h index f0bdb58..8c780ce 100644 --- a/libsensors/mlsdk/mllite/compass.h +++ b/libsensors/mlsdk/mllite/compass.h @@ -30,9 +30,6 @@ extern "C" { #include "mltypes.h" #include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "compass_legacy.h" -#endif /* ------------ */ /* - Defines. - */ /* ------------ */ @@ -74,14 +71,10 @@ typedef struct { /* --------------------- */ unsigned char inv_compass_present(void); - unsigned char inv_get_compass_slave_addr(void); inv_error_t inv_get_compass_data(long *data); inv_error_t inv_set_compass_bias(long *bias); - unsigned short inv_get_compass_id(void); inv_error_t inv_set_compass_offset(void); inv_error_t inv_compass_check_range(void); - inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val); - inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val); inv_error_t inv_compass_read_scale(long *val); int yas_filter_init(yas_filter_if_s *f); diff --git a/libsensors/mlsdk/mllite/dmpDefaultMantis.c b/libsensors/mlsdk/mllite/dmpDefaultMantis.c deleted file mode 100644 index 5282dd9..0000000 --- a/libsensors/mlsdk/mllite/dmpDefaultMantis.c +++ /dev/null @@ -1,467 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/* WARNING: autogenerated code, do not modify */ -/** - * @defgroup DMPDEFAULT - * @brief - * - * @{ - * @file inv_setup_dmpMantis.c - * @brief - * - * - */ - -#include "mltypes.h" -#include "dmpDefault.h" -#include "dmpKey.h" -#include "dmpmap.h" -#include "mldl.h" - -#define CFG_25 1786 -#define CFG_24 1782 -#define CFG_26 1790 -#define CFG_21 1899 -#define CFG_20 1728 -#define CFG_23 1911 -#define CFG_TAP4 1905 -#define CFG_TAP5 1906 -#define CFG_TAP6 1907 -#define CFG_1 1865 -#define CFG_TAP0 1899 -#define CFG_TAP1 1901 -#define CFG_TAP2 1902 -#define CFG_TAP_SAVE_ACCB 1770 -#define CFG_ORIENT_IRQ_1 1798 -#define CFG_ORIENT_IRQ_2 1821 -#define CFG_ORIENT_IRQ_3 1826 -#define FCFG_MAG_VAL 774 -#define CFG_TAP_QUANTIZE 1730 -#define FCFG_3 936 -#define FCFG_1 891 -#define CFG_ACCEL_FILTER 1076 -#define FCFG_2 895 -#define CFG_3D 1629 -#define FCFG_7 902 -#define CFG_3C 1627 -#define FCFG_5 1030 -#define FCFG_4 880 -#define CFG_3B 1625 -#define CFG_TAP_JERK 1722 -#define FCFG_6 954 -#define CFG_12 1894 -#define CFG_TAP7 1908 -#define CFG_14 1871 -#define CFG_15 1876 -#define CFG_16 1912 -#define CFG_TAP_CLEAR_STICKY 1914 -#define CFG_6 1920 -#define CFG_7 1014 -#define CFG_4 1634 -#define CFG_5 1831 -#define CFG_3 1623 -#define CFG_GYRO_SOURCE 1859 -#define CFG_TAP3 1903 -#define CFG_EXTERNAL 1884 -#define CFG_8 1854 -#define CFG_9 1860 -#define CFG_ORIENT_2 1816 -#define CFG_ORIENT_1 1796 -#define CFG_MOTION_BIAS 1023 -#define FCFG_MAG_MOV 791 -#define TEMPLABEL 1491 -#define FCFG_ACCEL_INIT 847 - - -#define D_0_22 (22+512) -#define D_0_24 (24+512) - -#define D_0_36 (36) -#define D_0_52 (52) -#define D_0_96 (96) -#define D_0_104 (104) -#define D_0_108 (108) -#define D_0_163 (163) -#define D_0_188 (188) -#define D_0_192 (192) -#define D_0_224 (224) -#define D_0_228 (228) -#define D_0_232 (232) -#define D_0_236 (236) - -#define D_1_2 (256 + 2) -#define D_1_4 (256 + 4) -#define D_1_8 (256 + 8) -#define D_1_10 (256 + 10) -#define D_1_24 (256 + 24) -#define D_1_28 (256 + 28) -#define D_1_92 (256 + 92) -#define D_1_96 (256 + 96) -#define D_1_98 (256 + 98) -#define D_1_106 (256 + 106) -#define D_1_108 (256 + 108) -#define D_1_112 (256 + 112) -#define D_1_128 (256 + 144) -#define D_1_152 (256 + 12) -#define D_1_168 (256 + 168) -#define D_1_175 (256 + 175) -#define D_1_178 (256 + 178) -#define D_1_236 (256 + 236) -#define D_1_244 (256 + 244) -#define D_2_12 (512+12) -#define D_2_96 (512+96) -#define D_2_108 (512+108) -#define D_2_244 (512+244) -#define D_2_248 (512+248) -#define D_2_252 (512+252) - -#define CPASS_BIAS_X (35*16+4) -#define CPASS_BIAS_Y (35*16+8) -#define CPASS_BIAS_Z (35*16+12) -#define CPASS_MTX_00 (36*16) -#define CPASS_MTX_01 (36*16+4) -#define CPASS_MTX_02 (36*16+8) -#define CPASS_MTX_10 (36*16+12) -#define CPASS_MTX_11 (37*16) -#define CPASS_MTX_12 (37*16+4) -#define CPASS_MTX_20 (37*16+8) -#define CPASS_MTX_21 (37*16+12) -#define CPASS_MTX_22 (43*16+12) -#define D_ACT0 (40*16) -#define D_ACSX (40*16+4) -#define D_ACSY (40*16+8) -#define D_ACSZ (40*16+12) - -static const tKeyLabel dmpTConfig[] = { - {KEY_CFG_25, CFG_25}, - {KEY_CFG_24, CFG_24}, - {KEY_CFG_26, CFG_26}, - {KEY_CFG_21, CFG_21}, - {KEY_CFG_20, CFG_20}, - {KEY_CFG_23, CFG_23}, - {KEY_CFG_TAP4, CFG_TAP4}, - {KEY_CFG_TAP5, CFG_TAP5}, - {KEY_CFG_TAP6, CFG_TAP6}, - {KEY_CFG_1, CFG_1}, - {KEY_CFG_TAP0, CFG_TAP0}, - {KEY_CFG_TAP1, CFG_TAP1}, - {KEY_CFG_TAP2, CFG_TAP2}, - {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB}, - {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1}, - {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2}, - {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3}, - {KEY_FCFG_MAG_VAL, FCFG_MAG_VAL}, - {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE}, - {KEY_FCFG_3, FCFG_3}, - {KEY_FCFG_1, FCFG_1}, -// {KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER}, - {KEY_FCFG_2, FCFG_2}, - {KEY_CFG_3D, CFG_3D}, - {KEY_FCFG_7, FCFG_7}, - {KEY_CFG_3C, CFG_3C}, - {KEY_FCFG_5, FCFG_5}, - {KEY_FCFG_4, FCFG_4}, - {KEY_CFG_3B, CFG_3B}, - {KEY_CFG_TAP_JERK, CFG_TAP_JERK}, - {KEY_FCFG_6, FCFG_6}, - {KEY_CFG_12, CFG_12}, - {KEY_CFG_TAP7, CFG_TAP7}, - {KEY_CFG_14, CFG_14}, - {KEY_CFG_15, CFG_15}, - {KEY_CFG_16, CFG_16}, - {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY}, - {KEY_CFG_6, CFG_6}, - {KEY_CFG_7, CFG_7}, - {KEY_CFG_4, CFG_4}, - {KEY_CFG_5, CFG_5}, - {KEY_CFG_3, CFG_3}, - {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE}, - {KEY_CFG_TAP3, CFG_TAP3}, - {KEY_CFG_EXTERNAL, CFG_EXTERNAL}, - {KEY_CFG_8, CFG_8}, - {KEY_CFG_9, CFG_9}, - {KEY_CFG_ORIENT_2, CFG_ORIENT_2}, - {KEY_CFG_ORIENT_1, CFG_ORIENT_1}, - {KEY_CFG_MOTION_BIAS, CFG_MOTION_BIAS}, - {KEY_FCFG_MAG_MOV, FCFG_MAG_MOV}, -// {KEY_TEMPLABEL, TEMPLABEL}, - {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT}, - {KEY_D_0_22, D_0_22}, - {KEY_D_0_24, D_0_24}, - {KEY_D_0_36, D_0_36}, - {KEY_D_0_52, D_0_52}, - {KEY_D_0_96, D_0_96}, - {KEY_D_0_104, D_0_104}, - {KEY_D_0_108, D_0_108}, - {KEY_D_0_163, D_0_163}, - {KEY_D_0_188, D_0_188}, - {KEY_D_0_192, D_0_192}, - {KEY_D_0_224, D_0_224}, - {KEY_D_0_228, D_0_228}, - {KEY_D_0_232, D_0_232}, - {KEY_D_0_236, D_0_236}, - - {KEY_DMP_PREVPTAT, DMP_PREVPTAT}, - {KEY_D_1_2, D_1_2}, - {KEY_D_1_4, D_1_4}, - {KEY_D_1_8, D_1_8}, - {KEY_D_1_10, D_1_10}, - {KEY_D_1_24, D_1_24}, - {KEY_D_1_28, D_1_28}, - {KEY_D_1_92, D_1_92}, - {KEY_D_1_96, D_1_96}, - {KEY_D_1_98, D_1_98}, - {KEY_D_1_106, D_1_106}, - {KEY_D_1_108, D_1_108}, - {KEY_D_1_112, D_1_112}, - {KEY_D_1_128, D_1_128}, - {KEY_D_1_152, D_1_152}, - {KEY_D_1_168, D_1_168}, - {KEY_D_1_175, D_1_175}, - {KEY_D_1_178, D_1_178}, - {KEY_D_1_236, D_1_236}, - {KEY_D_1_244, D_1_244}, - - {KEY_D_2_12, D_2_12}, - {KEY_D_2_96, D_2_96}, - {KEY_D_2_108, D_2_108}, - {KEY_D_2_244, D_2_244}, - {KEY_D_2_248, D_2_248}, - {KEY_D_2_252, D_2_252}, - - {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN}, - {KEY_DMP_TAP_THR_X, DMP_TAP_THX}, - {KEY_DMP_TAP_THR_Y, DMP_TAP_THY}, - {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ}, - {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y}, - {KEY_DMP_SH_TH_X, DMP_SH_TH_X}, - {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z}, - {KEY_DMP_ORIENT, DMP_ORIENT}, - - {KEY_CPASS_BIAS_X, CPASS_BIAS_X}, - {KEY_CPASS_BIAS_Y, CPASS_BIAS_Y}, - {KEY_CPASS_BIAS_Z, CPASS_BIAS_Z}, - {KEY_CPASS_MTX_00, CPASS_MTX_00}, - {KEY_CPASS_MTX_01, CPASS_MTX_01}, - {KEY_CPASS_MTX_02, CPASS_MTX_02}, - {KEY_CPASS_MTX_10, CPASS_MTX_10}, - {KEY_CPASS_MTX_11, CPASS_MTX_11}, - {KEY_CPASS_MTX_12, CPASS_MTX_12}, - {KEY_CPASS_MTX_20, CPASS_MTX_20}, - {KEY_CPASS_MTX_21, CPASS_MTX_21}, - {KEY_CPASS_MTX_22, CPASS_MTX_22}, - {KEY_D_ACT0, D_ACT0}, - {KEY_D_ACSX, D_ACSX}, - {KEY_D_ACSY, D_ACSY}, - {KEY_D_ACSZ, D_ACSZ} -}; -#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) - -#define DMP_CODE_SIZE 1923 - -static const unsigned char dmpMemory[DMP_CODE_SIZE] = { - 0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xff, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x2a, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xfd, 0x87, 0x26, 0x50, 0xfd, 0x80, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6f, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5e, 0xc0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc, - 0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4, - 0xff, 0xe6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10, - 0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8, - 0xff, 0xd0, 0x9a, 0xbe, 0x00, 0x00, 0x10, 0xa9, 0xff, 0xf4, 0x1e, 0xb2, 0x00, 0xce, 0xbb, 0xf7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0c, - 0xff, 0xc2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xcf, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3f, 0x68, 0xb6, 0x79, 0x35, 0x28, 0xbc, 0xc6, 0x7e, 0xd1, 0x6c, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 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, - 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xff, 0xef, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - 0xd8, 0xdc, 0xba, 0xa2, 0xf1, 0xde, 0xb2, 0xb8, 0xb4, 0xa8, 0x81, 0x91, 0xf7, 0x4a, 0x90, 0x7f, - 0x91, 0x6a, 0xf3, 0xf9, 0xdb, 0xa8, 0xf9, 0xb0, 0xba, 0xa0, 0x80, 0xf2, 0xce, 0x81, 0xf3, 0xc2, - 0xf1, 0xc1, 0xf2, 0xc3, 0xf3, 0xcc, 0xa2, 0xb2, 0x80, 0xf1, 0xc6, 0xd8, 0x80, 0xba, 0xa7, 0xdf, - 0xdf, 0xdf, 0xf2, 0xa7, 0xc3, 0xcb, 0xc5, 0xb6, 0xf0, 0x87, 0xa2, 0x94, 0x24, 0x48, 0x70, 0x3c, - 0x95, 0x40, 0x68, 0x34, 0x58, 0x9b, 0x78, 0xa2, 0xf1, 0x83, 0x92, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, - 0xb4, 0xb8, 0xa1, 0xd0, 0x91, 0x80, 0xf2, 0x70, 0xf3, 0x70, 0xf2, 0x7c, 0x80, 0xa8, 0xf1, 0x01, - 0xb0, 0x98, 0x87, 0xd9, 0x43, 0xd8, 0x86, 0xc9, 0x88, 0xba, 0xa1, 0xf2, 0x0e, 0xb8, 0x97, 0x80, - 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0xc1, - 0xc9, 0xc3, 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, 0xb9, 0xaf, 0xb4, 0xb0, - 0x83, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb1, 0x8f, 0x98, 0xb9, 0xaf, 0xf0, 0x24, 0x08, 0x44, 0x10, - 0x64, 0x18, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xaf, 0x83, 0xb5, 0x93, 0xaf, 0xf0, 0x00, 0x28, 0x50, - 0xf1, 0xa3, 0x86, 0x9f, 0x61, 0xa6, 0xda, 0xde, 0xdf, 0xd9, 0xfa, 0xa3, 0x86, 0x96, 0xdb, 0x31, - 0xa6, 0xd9, 0xf8, 0xdf, 0xba, 0xa6, 0x8f, 0xc2, 0xc5, 0xc7, 0xb2, 0x8c, 0xc1, 0xb8, 0xa2, 0xdf, - 0xdf, 0xdf, 0xa3, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb2, 0x86, 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, 0xb9, - 0xa3, 0xde, 0xdf, 0xdf, 0xa3, 0xb1, 0x80, 0xf2, 0xc4, 0xcd, 0xc9, 0xf1, 0xb8, 0xa9, 0xb4, 0x99, - 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0xb5, 0x93, 0xa3, 0x0e, 0x16, 0x1e, - 0xa9, 0x2c, 0x54, 0x7c, 0xb8, 0xb4, 0xb0, 0xf1, 0x97, 0x83, 0xa8, 0x11, 0x84, 0xa5, 0x09, 0x98, - 0xa3, 0x83, 0xf0, 0xda, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xd8, 0xf1, 0xa5, 0x29, 0x55, 0x7d, - 0xa5, 0x85, 0x95, 0x02, 0x1a, 0x2e, 0x3a, 0x56, 0x5a, 0x40, 0x48, 0xf9, 0xf3, 0xa3, 0xd9, 0xf8, - 0xf0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xa8, 0xf1, 0x11, 0xf0, 0x98, - 0xa2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xda, 0xf3, 0xde, 0xd8, 0x83, 0xa5, 0x94, 0x01, 0xd9, - 0xa3, 0x02, 0xf1, 0xa2, 0xc3, 0xc5, 0xc7, 0xd8, 0xf1, 0x84, 0x92, 0xa2, 0x4d, 0xda, 0x2a, 0xd8, - 0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, - 0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0x93, 0xa3, 0x4d, 0xda, 0x2a, 0xd8, - 0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, - 0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0xa8, 0x8a, 0x9a, 0xf0, 0x28, 0x50, - 0x78, 0x9e, 0xf3, 0x88, 0x18, 0xf1, 0x9f, 0x1d, 0x98, 0xa8, 0xd9, 0x08, 0xd8, 0xc8, 0x9f, 0x12, - 0x9e, 0xf3, 0x15, 0xa8, 0xda, 0x12, 0x10, 0xd8, 0xf1, 0xaf, 0xc8, 0x97, 0x87, 0x34, 0xb5, 0xb9, - 0x94, 0xa4, 0x21, 0xf3, 0xd9, 0x22, 0xd8, 0xf2, 0x2d, 0xf3, 0xd9, 0x2a, 0xd8, 0xf2, 0x35, 0xf3, - 0xd9, 0x32, 0xd8, 0x81, 0xa4, 0x60, 0x60, 0x61, 0xd9, 0x61, 0xd8, 0x6c, 0x68, 0x69, 0xd9, 0x69, - 0xd8, 0x74, 0x70, 0x71, 0xd9, 0x71, 0xd8, 0xb1, 0xa3, 0x84, 0x19, 0x3d, 0x5d, 0xa3, 0x83, 0x1a, - 0x3e, 0x5e, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xb8, 0xb0, 0xaf, 0x8f, 0x94, 0xf2, 0xda, 0x3e, - 0xd8, 0xb4, 0x9a, 0xa8, 0x87, 0x29, 0xda, 0xf8, 0xd8, 0x87, 0x9a, 0x35, 0xda, 0xf8, 0xd8, 0x87, - 0x9a, 0x3d, 0xda, 0xf8, 0xd8, 0xb1, 0xb9, 0xa4, 0x98, 0x85, 0x02, 0x2e, 0x56, 0xa5, 0x81, 0x00, - 0x0c, 0x14, 0xa3, 0x97, 0xb0, 0x8a, 0xf1, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, - 0xd9, 0x68, 0xd8, 0xb1, 0x84, 0x0d, 0xda, 0x0e, 0xd8, 0xa3, 0x29, 0x83, 0xda, 0x2c, 0x0e, 0xd8, - 0xa3, 0x84, 0x49, 0x83, 0xda, 0x2c, 0x4c, 0x0e, 0xd8, 0xb8, 0xb0, 0xa8, 0x8a, 0x9a, 0xf5, 0x20, - 0xaa, 0xda, 0xdf, 0xd8, 0xa8, 0x40, 0xaa, 0xd0, 0xda, 0xde, 0xd8, 0xa8, 0x60, 0xaa, 0xda, 0xd0, - 0xdf, 0xd8, 0xf1, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, - 0xf0, 0x0c, 0x20, 0x14, 0x40, 0xb8, 0xb0, 0xb4, 0xa8, 0x8c, 0x9c, 0xf0, 0x04, 0x28, 0x51, 0x79, - 0x1d, 0x30, 0x14, 0x38, 0xb2, 0x82, 0xab, 0xd0, 0x98, 0x2c, 0x50, 0x50, 0x78, 0x78, 0x9b, 0xf1, - 0x1a, 0xb0, 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, 0xa5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, - 0x49, 0x30, 0x19, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, - 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, - 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, - 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, - 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, - 0x88, 0x2d, 0x55, 0x7d, 0x9e, 0xb9, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, - 0xb1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xb8, 0xad, 0x00, 0x2c, 0x54, - 0x7c, 0xf2, 0xb1, 0x8c, 0xb4, 0x99, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0x81, 0x91, 0xac, 0x38, 0xad, - 0x3a, 0xb5, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, - 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9, 0x04, 0xae, 0xd8, 0x79, 0xd9, - 0x04, 0xd8, 0x81, 0xf3, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81, 0xad, 0xd9, 0x01, 0xd8, 0xf2, - 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad, 0xad, 0xad, 0xad, 0xf3, 0x2a, - 0xd8, 0xd8, 0xf1, 0xb0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xf3, 0xac, 0x2e, 0x2e, 0xf1, 0xb1, - 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30, 0x18, 0xa8, 0x98, 0x81, 0x28, - 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xf2, 0xb0, 0x89, 0xac, 0x91, 0x2c, - 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8, 0xd8, 0x79, 0xd9, 0xd8, 0xd8, - 0xf1, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, - 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xb1, 0x83, 0x93, 0x35, 0x3d, 0x80, 0x25, 0xda, 0xd8, 0xd8, 0x85, - 0x69, 0xda, 0xd8, 0xd8, 0xb4, 0x93, 0x81, 0xa3, 0x28, 0x34, 0x3c, 0xf3, 0xab, 0x8b, 0xf8, 0xa3, - 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xfa, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xa3, - 0xa3, 0xa3, 0xa3, 0x95, 0xf1, 0xa3, 0xa3, 0xa3, 0x9d, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, 0xf2, 0xa3, - 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xb2, 0xa3, 0xa3, 0xa3, 0xa3, - 0xa3, 0xa3, 0xb0, 0x87, 0xb5, 0x99, 0xf1, 0xa3, 0xa3, 0xa3, 0x98, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, - 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xb9, 0xa7, 0xf1, 0x26, 0x26, 0x26, - 0xd8, 0xd8, 0xff -}; - -static unsigned short sStartAddress = 0x0300; - - -static tKeyLabel keys[NUM_KEYS]; - -static unsigned short inv_setup_dmpGetAddress( unsigned short key ) -{ - static int isSorted = 0; - if ( !isSorted ) { - int kk; - for (kk=0; kk<NUM_KEYS; ++kk) { - keys[ kk ].addr = 0xffff; - keys[ kk ].key = kk; - } - for (kk=0; kk<NUM_LOCAL_KEYS; ++kk) { - keys[ dmpTConfig[kk].key ].addr = dmpTConfig[kk].addr; - } - isSorted = 1; - } - if ( key >= NUM_KEYS ) - return 0xffff; - return keys[ key ].addr; -} - -/** - * @brief - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_setup_dmp(void) - -{ - inv_error_t result; - - inv_set_get_address(inv_setup_dmpGetAddress); - result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_full_scale(2000.f); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_load_dmp(dmpMemory, DMP_CODE_SIZE, sStartAddress); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_external_sync(MPU_EXT_SYNC_TEMP); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} -/** - *@} - */ diff --git a/libsensors/mlsdk/mllite/invensense.h b/libsensors/mlsdk/mllite/invensense.h index 586dd25..423aba7 100644 --- a/libsensors/mlsdk/mllite/invensense.h +++ b/libsensors/mlsdk/mllite/invensense.h @@ -12,7 +12,6 @@ #include "mlMathFunc.h" #include "mlSetGyroBias.h" #include "ml_legacy.h" -#include "ml_mputest.h" #include "ml_stored_data.h" #include "mlcontrol.h" #include "mldl.h" @@ -21,4 +20,3 @@ #include "mlinclude.h" #include "mlstates.h" #include "mlsupervisor.h" -#include "pressure.h" diff --git a/libsensors/mlsdk/mllite/ml.c b/libsensors/mlsdk/mllite/ml.c index d830be7..c0f9a37 100644 --- a/libsensors/mlsdk/mllite/ml.c +++ b/libsensors/mlsdk/mllite/ml.c @@ -705,34 +705,6 @@ inv_error_t inv_set_fifo_interrupt(unsigned char on) } /** - * @brief Get the current set of DMP interrupt sources. - * These interrupts are generated by the DMP and can be - * routed to the MPU interrupt line via internal - * settings. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @return Currently enabled interrupt sources. The possible interrupts are: - * - INV_INT_FIFO, - * - INV_INT_MOTION, - * - INV_INT_TAP - */ -int inv_get_interrupts(void) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return 0; // error - - return inv_obj.interrupt_sources; -} - -/** * @brief Sets up the Accelerometer calibration and scale factor. * * Please refer to the provided "9-Axis Sensor Fusion Application @@ -1044,14 +1016,6 @@ inv_error_t inv_set_compass_calibration(float range, signed char *orientation) float cal[9]; float scale = range / 32768.f; int kk; - unsigned short compassId = 0; - - compassId = inv_get_compass_id(); - - if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883) - || (compassId == COMPASS_ID_LSM303M)) { - scale /= 32.0f; - } for (kk = 0; kk < 9; ++kk) { cal[kk] = scale * orientation[kk]; @@ -1262,28 +1226,6 @@ inv_error_t inv_set_bias_update(unsigned short function) } /** - * @brief inv_get_motion_state is used to determine if the device is in - * a 'motion' or 'no motion' state. - * inv_get_motion_state returns INV_MOTION of the device is moving, - * or INV_NO_MOTION if the device is not moving. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() - * must have been called. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -int inv_get_motion_state(void) -{ - INVENSENSE_FUNC_START; - return inv_obj.motion_state; -} - -/** * @brief inv_set_no_motion_thresh is used to set the threshold for * detecting INV_NO_MOTION * @@ -1395,25 +1337,6 @@ inv_error_t inv_set_no_motion_time(float time) } /** - * @brief inv_get_version is used to get the ML version. - * - * @pre inv_get_version can be called at any time. - * - * @param version inv_get_version writes the ML version - * string pointer to version. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_get_version(unsigned char **version) -{ - INVENSENSE_FUNC_START; - - *version = (unsigned char *)mlVer; //fixme we are wiping const - - return INV_SUCCESS; -} - -/** * @brief Check for the presence of the gyro sensor. * * This is not a physical check but a logical check and the value can change diff --git a/libsensors/mlsdk/mllite/ml.h b/libsensors/mlsdk/mllite/ml.h index 67e47e2..0104b53 100644 --- a/libsensors/mlsdk/mllite/ml.h +++ b/libsensors/mlsdk/mllite/ml.h @@ -46,9 +46,6 @@ extern "C" { #include "mldmp.h" #include "mlsl.h" #include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "ml_legacy.h" -#endif /* ------------ */ /* - Defines. - */ @@ -140,70 +137,6 @@ extern "C" { #define INV_YAW (INV_Z_AXIS) /*************************************************************************/ -/* Sensor types */ -/*************************************************************************/ -#define INV_GYROS 0x0001 -#define INV_ACCELS 0x0002 - -/*************************************************************************/ -/* Motion arrays */ -/*************************************************************************/ -#define INV_ROTATION_MATRIX 0x0003 -#define INV_QUATERNION 0x0004 -#define INV_EULER_ANGLES 0x0005 -#define INV_LINEAR_ACCELERATION 0x0006 -#define INV_LINEAR_ACCELERATION_WORLD 0x0007 -#define INV_GRAVITY 0x0008 -#define INV_ANGULAR_VELOCITY 0x0009 - -#define INV_GYRO_CALIBRATION_MATRIX 0x000B -#define INV_ACCEL_CALIBRATION_MATRIX 0x000C -#define INV_GYRO_BIAS 0x000D -#define INV_ACCEL_BIAS 0x000E -#define INV_GYRO_TEMP_SLOPE 0x000F - -#define INV_RAW_DATA 0x0011 -#define INV_DMP_TAP 0x0012 -#define INV_DMP_TAP2 0x0021 - -#define INV_EULER_ANGLES_X 0x0013 -#define INV_EULER_ANGLES_Y 0x0014 -#define INV_EULER_ANGLES_Z 0x0015 - -#define INV_BIAS_UNCERTAINTY 0x0016 -#define INV_DMP_PACKET_NUMBER 0x0017 -#define INV_FOOTER 0x0018 - -#define INV_CONTROL_DATA 0x0019 - -#define INV_MAGNETOMETER 0x001A -#define INV_PEDLBS 0x001B -#define INV_MAG_RAW_DATA 0x001C -#define INV_MAG_CALIBRATION_MATRIX 0x001D -#define INV_MAG_BIAS 0x001E -#define INV_HEADING 0x001F - -#define INV_MAG_BIAS_ERROR 0x0020 - -#define INV_PRESSURE 0x0021 -#define INV_LOCAL_FIELD 0x0022 -#define INV_MAG_SCALE 0x0023 - -#define INV_RELATIVE_QUATERNION 0x0024 - -#define SET_QUATERNION 0x0001 -#define SET_GYROS 0x0002 -#define SET_LINEAR_ACCELERATION 0x0004 -#define SET_GRAVITY 0x0008 -#define SET_ACCELS 0x0010 -#define SET_TAP 0x0020 -#define SET_PEDLBS 0x0040 -#define SET_LINEAR_ACCELERATION_WORLD 0x0080 -#define SET_CONTROL 0x0100 -#define SET_PACKET_NUMBER 0x4000 -#define SET_FOOTER 0x8000 - -/*************************************************************************/ /* Integral reset options */ /*************************************************************************/ #define INV_NO_RESET 0x0000 @@ -459,8 +392,6 @@ struct inv_obj_t { /* Legacy functions for handling augmented data*/ inv_error_t inv_get_array(int dataSet, long *data); inv_error_t inv_get_float_array(int dataSet, float *data); - inv_error_t inv_set_array(int dataSet, long *data); - inv_error_t inv_set_float_array(int dataSet, float *data); /* Individual functions for augmented data, per specific dataset */ @@ -550,14 +481,9 @@ struct inv_obj_t { inv_set_motion_callback(void (*func) (unsigned short motion_state)); int inv_get_motion_state(void); - /*API for getting ML version. */ - inv_error_t inv_get_version(unsigned char **version); - inv_error_t inv_set_motion_interrupt(unsigned char on); inv_error_t inv_set_fifo_interrupt(unsigned char on); - int inv_get_interrupts(void); - /* Simulated DMP */ int inv_get_gyro_present(void); @@ -566,7 +492,6 @@ struct inv_obj_t { inv_error_t inv_set_no_motion_threshAccel(long thresh); inv_error_t inv_reset_motion(void); - inv_error_t inv_update_bias(void); inv_error_t inv_set_dead_zone(void); void inv_start_bias_calc(void); void inv_stop_bias_calc(void); diff --git a/libsensors/mlsdk/mllite/mlBiasNoMotion.c b/libsensors/mlsdk/mllite/mlBiasNoMotion.c index aaf98d2..2cec85c 100644 --- a/libsensors/mlsdk/mllite/mlBiasNoMotion.c +++ b/libsensors/mlsdk/mllite/mlBiasNoMotion.c @@ -71,7 +71,7 @@ inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state)) return INV_SUCCESS; } -inv_error_t inv_update_bias(void) +static inv_error_t inv_update_bias(void) { INVENSENSE_FUNC_START; inv_error_t result; @@ -127,7 +127,7 @@ inv_error_t inv_update_bias(void) return INV_SUCCESS; } -inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) +static inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) { long gain; unsigned long timeChange; @@ -171,14 +171,18 @@ inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) inv_obj->no_motion_accel_time = currentTime; // Check for change of state - if (!inv_get_gyro_present()) + + // TODO: With !gyro check in place, motion callbacks are NEVER fired? + //if (!inv_get_gyro_present()) inv_set_motion_state(INV_MOTION); } else if ((currentTime - inv_obj->no_motion_accel_time) > 5 * inv_obj->motion_duration) { // We have no motion according to accel - // Check fsor change of state - if (!inv_get_gyro_present()) + // Check for change of state + + // TODO: With !gyro check in place, motion callbacks are NEVER fired? + //if (!inv_get_gyro_present()) inv_set_motion_state(INV_NO_MOTION); } } @@ -194,7 +198,7 @@ inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) * 'no motion' state and update the internal motion status and bias * calculations. */ -inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) +static inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) { INVENSENSE_FUNC_START; unsigned char regs[3] = { 0 }; @@ -225,9 +229,8 @@ inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; - _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); - ) - if (motionFlag == inv_obj->motion_duration) { + _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag);) + if (motionFlag == inv_obj->motion_duration) { if (inv_obj->motion_state == INV_MOTION) { inv_update_bias(); repeatBiasUpdateTime = inv_get_tick_count(); diff --git a/libsensors/mlsdk/mllite/mlFIFO.c b/libsensors/mlsdk/mllite/mlFIFO.c index 3279b35..4bb9a39 100644 --- a/libsensors/mlsdk/mllite/mlFIFO.c +++ b/libsensors/mlsdk/mllite/mlFIFO.c @@ -661,12 +661,6 @@ inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, } } - result = inv_pressure_supervisor(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - // Callbacks now that we have a buffer of data ready result = inv_run_fifo_rate_processes(); if (result) { diff --git a/libsensors/mlsdk/mllite/mlFIFO.h b/libsensors/mlsdk/mllite/mlFIFO.h index 2114eb3..7299d2f 100644 --- a/libsensors/mlsdk/mllite/mlFIFO.h +++ b/libsensors/mlsdk/mllite/mlFIFO.h @@ -22,9 +22,6 @@ #include "mltypes.h" #include "mlinclude.h" #include "ml.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlFIFO_legacy.h" -#endif #ifdef __cplusplus extern "C" { diff --git a/libsensors/mlsdk/mllite/mlFIFOHW.h b/libsensors/mlsdk/mllite/mlFIFOHW.h index 6f70185..9d42caa 100644 --- a/libsensors/mlsdk/mllite/mlFIFOHW.h +++ b/libsensors/mlsdk/mllite/mlFIFOHW.h @@ -21,9 +21,6 @@ #include "mpu.h" #include "mltypes.h" #include "mlinclude.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlFIFOHW_legacy.h" -#endif #ifdef __cplusplus extern "C" { diff --git a/libsensors/mlsdk/mllite/mlMathFunc.c b/libsensors/mlsdk/mllite/mlMathFunc.c index 31b42bc..b975213 100644 --- a/libsensors/mlsdk/mllite/mlMathFunc.c +++ b/libsensors/mlsdk/mllite/mlMathFunc.c @@ -19,48 +19,6 @@ #include "mlMathFunc.h" #include "mlinclude.h" -/** - * Performs one iteration of the filter, generating a new y(0) - * 1 / N / N \\ - * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length - * a(0) \k=0 \ k=1 // - * - * The filters A and B should be (sizeof(long) * state->length). - * The state variables x and y should be (sizeof(long) * (state->length - 1)) - * - * The state variables x and y should be initialized prior to the first call - * - * @param state Contains the length of the filter, filter coefficients and - * filter state variables x and y. - * @param x New input into the filter. - */ -void inv_filter_long(struct filter_long *state, long x) -{ - const long *b = state->b; - const long *a = state->a; - long length = state->length; - long long tmp; - int ii; - - /* filter */ - tmp = (long long)x *(b[0]); - for (ii = 0; ii < length - 1; ii++) { - tmp += ((long long)state->x[ii] * (long long)(b[ii + 1])); - } - for (ii = 0; ii < length - 1; ii++) { - tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1])); - } - tmp /= (long long)a[0]; - - /* Delay */ - for (ii = length - 2; ii > 0; ii--) { - state->y[ii] = state->y[ii - 1]; - state->x[ii] = state->x[ii - 1]; - } - /* New values */ - state->y[0] = (long)tmp; - state->x[0] = x; -} /** 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 @@ -111,36 +69,6 @@ void inv_q_mult(const long *q1, const long *q2, long *qProd) (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 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_q_normalize(long *q) -{ - INVENSENSE_FUNC_START; - double normSF = 0; - int i; - for (i = 0; i < 4; i++) { - normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L; - } - if (normSF > 0) { - normSF = 1 / sqrt(normSF); - for (i = 0; i < 4; i++) { - q[i] = (int)((double)q[i] * normSF); - } - } else { - q[0] = 1073741824L; - q[1] = 0; - q[2] = 0; - q[3] = 0; - } -} - void inv_q_invert(const long *q, long *qInverted) { INVENSENSE_FUNC_START; @@ -150,78 +78,6 @@ void inv_q_invert(const long *q, long *qInverted) qInverted[3] = -q[3]; } -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(float *q1, 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] 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. @@ -292,21 +148,6 @@ void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) *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 + 10 * k + l) = *(a + 10 * i + j); - } - } - *n = *n - 1; -} - float inv_matrix_det(float *p, int *n) { float d[10][10], sum = 0; @@ -325,24 +166,6 @@ float inv_matrix_det(float *p, int *n) return (sum); } -double inv_matrix_detd(double *p, int *n) -{ - double d[10][10], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 11) - *(p + 1) ** (p + 10)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_incd(p, &d[0][0], n, i, j); - sum = - sum + *(p + 10 * 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] diff --git a/libsensors/mlsdk/mllite/mlMathFunc.h b/libsensors/mlsdk/mllite/mlMathFunc.h index 70fa9f4..475cfc7 100644 --- a/libsensors/mlsdk/mllite/mlMathFunc.h +++ b/libsensors/mlsdk/mllite/mlMathFunc.h @@ -17,48 +17,23 @@ */ #ifndef INVENSENSE_INV_MATH_FUNC_H__ #define INVENSENSE_INV_MATH_FUNC_H__ -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlMathFunc_legacy.h" -#endif -#define NUM_ROTATION_MATRIX_ELEMENTS (9) -#define ROT_MATRIX_SCALE_LONG (1073741824) -#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) #ifdef __cplusplus extern "C" { #endif - struct filter_long { - int length; - const long *b; - const long *a; - long *x; - long *y; - }; - void inv_filter_long(struct filter_long *state, long x); long inv_q29_mult(long a, long b); long inv_q30_mult(long a, long b); 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(float *q1, 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); 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); diff --git a/libsensors/mlsdk/mllite/ml_mputest.c b/libsensors/mlsdk/mllite/ml_mputest.c deleted file mode 100644 index ffb17cd..0000000 --- a/libsensors/mlsdk/mllite/ml_mputest.c +++ /dev/null @@ -1,180 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: ml_mputest.c 5641 2011-06-14 02:10:02Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup MPU_SELF_TEST - * @brief C wrapper to integrate the MPU Self Test wrapper in MPL. - * Provides ML name compliant naming and an additional API that - * automates the suspension of normal MPL operations, runs the test, - * and resume. - * - * @{ - * @file ml_mputest.c - * @brief C wrapper to integrate the MPU Self Test wrapper in MPL. - * The main logic of the test and APIs can be found in mputest.c - */ - -#include <stdio.h> -#include <time.h> -#include <string.h> -#include <math.h> -#include <stdlib.h> - -#include "ml_mputest.h" - -#include "mlmath.h" -#include "mlinclude.h" -#include "ml.h" -#include "mlstates.h" -#include "mldl.h" -#include "mldl_cfg.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Globals -*/ -extern struct mldl_cfg *mputestCfgPtr; -extern signed char g_z_sign; - -/* - Prototypes -*/ -extern inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result); - -/** - * @brief An MPL wrapper for the main MPU Self Test API inv_factory_calibrate(). - * See inv_factory_calibrate() function for more details. - * - * @pre inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg - * data structure. - * On Windows, SetupPlatform() is also a madatory pre condition to - * ensure the accelerometer is properly configured before running the - * test. - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set thresholds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro_xxxx() functions. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * NOTE: for MPU6050 devices, this parameter is currently - * ignored. - * - * @return INV_SUCCESS or first non-zero error code otherwise. - */ -inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle, - unsigned char provide_result) -{ - INVENSENSE_FUNC_START; - inv_error_t firstError = INV_SUCCESS; - inv_error_t result; - unsigned char initState = inv_get_state(); - - if (initState < INV_STATE_DMP_OPENED) { - MPL_LOGE("Self Test cannot run before inv_dmp_open()\n"); - return INV_ERROR_SM_IMPROPER_STATE; - } - - /* obtain a pointer to the 'struct mldl_cfg' data structure. */ - mputestCfgPtr = inv_get_dl_config(); - - if(initState == INV_STATE_DMP_STARTED) { - result = inv_dmp_stop(); - ERROR_CHECK_FIRST(firstError, result); - } - - result = inv_factory_calibrate(mlsl_handle, provide_result); - ERROR_CHECK_FIRST(firstError, result); - - if(initState == INV_STATE_DMP_STARTED) { - result = inv_dmp_start(); - ERROR_CHECK_FIRST(firstError, result); - } - - return firstError; -} - -/** - * @brief Runs the MPU test at MPL runtime. - * If the DMP is operating, stops the DMP temporarely, - * runs the MPU Self Test, and re-starts the DMP. - * - * @return INV_SUCCESS or a non-zero error code otherwise. - */ -inv_error_t inv_self_test_run(void) -{ - return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE); -} - -/** - * @brief Runs the MPU test for bias correction only at MPL runtime. - * If the DMP is operating, stops the DMP temporarely, - * runs the bias calculation routines, and re-starts the DMP. - * - * @return INV_SUCCESS or a non-zero error code otherwise. - */ -inv_error_t inv_self_test_bias_only(void) -{ - return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); -} - -/** - * @brief Set the orientation of the acceleroemter Z axis as it will be - * expected when running the MPU Self Test. - * Specifies the orientation of the accelerometer Z axis : Z axis - * pointing upwards or downwards. - * @param z_sign - * The sign of the accelerometer Z axis; valid values are +1 and - * -1 for +Z and -Z respectively. Any other value will cause the - * setting to be ignored and an error code to be returned. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign) -{ - if (z_sign != +1 && z_sign != -1) { - return INV_ERROR_INVALID_PARAMETER; - } - g_z_sign = z_sign; - return INV_SUCCESS; -} - - -#ifdef __cplusplus -} -#endif - -/** - * @} - */ - diff --git a/libsensors/mlsdk/mllite/ml_mputest.h b/libsensors/mlsdk/mllite/ml_mputest.h deleted file mode 100644 index 705d3cc..0000000 --- a/libsensors/mlsdk/mllite/ml_mputest.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: ml_mputest.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *****************************************************************************/ - -#ifndef _INV_MPUTEST_H_ -#define _INV_MPUTEST_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" - -/* user APIs */ -inv_error_t inv_self_test_factory_calibrate( - void *mlsl_handle, unsigned char provide_result); -inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign); -inv_error_t inv_self_test_run(void); -inv_error_t inv_self_test_bias_only(void); - -/* other functions */ -#define inv_set_self_test_parameters inv_set_test_parameters - -#ifdef __cplusplus -} -#endif - -#endif /* _INV_MPUTEST_H_ */ - diff --git a/libsensors/mlsdk/mllite/ml_stored_data.c b/libsensors/mlsdk/mllite/ml_stored_data.c index 35da3e8..7619c21 100644 --- a/libsensors/mlsdk/mllite/ml_stored_data.c +++ b/libsensors/mlsdk/mllite/ml_stored_data.c @@ -342,9 +342,9 @@ inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len) accelBias[2] = (long)((long long)accelBias[2] * 65536L * inv_obj.accel_sens / 1073741824L); LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]); - if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); - return inv_set_array(INV_ACCEL_BIAS, accelBias); + if (inv_set_accel_bias(accelBias)) { + LOG_RESULT_LOCATION(inv_set_accel_bias(accelBias)); + return inv_set_accel_bias(accelBias); } inv_obj.got_no_motion_bias = TRUE; @@ -494,9 +494,9 @@ inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len) LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]); } - if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias)); - return inv_set_array(INV_ACCEL_BIAS, accel_bias); + if (inv_set_accel_bias(accel_bias)) { + LOG_RESULT_LOCATION(inv_set_accel_bias(accel_bias)); + return inv_set_accel_bias(accel_bias); } inv_obj.got_no_motion_bias = TRUE; @@ -651,9 +651,9 @@ inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len) bias[i] = (int32_t) t; LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); } - if (inv_set_array(INV_ACCEL_BIAS, bias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); - return inv_set_array(INV_ACCEL_BIAS, bias); + if (inv_set_accel_bias(bias)) { + LOG_RESULT_LOCATION(inv_set_accel_bias(bias)); + return inv_set_accel_bias(bias); } /* read the compass biases */ @@ -882,9 +882,9 @@ inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len) bias[i] = (int32_t) t; LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); } - if (inv_set_array(INV_ACCEL_BIAS, bias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); - return inv_set_array(INV_ACCEL_BIAS, bias); + if (inv_set_accel_bias(bias)) { + LOG_RESULT_LOCATION(inv_set_accel_bias(bias)); + return inv_set_accel_bias(bias); } /* read the compass biases */ @@ -1127,9 +1127,9 @@ inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len) accelBias[i] = (long)tmp; LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]); } - if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); - return inv_set_array(INV_ACCEL_BIAS, accelBias); + if (inv_set_accel_bias(accelBias)) { + LOG_RESULT_LOCATION(inv_set_accel_bias(accelBias)); + return inv_set_accel_bias(accelBias); } inv_obj.got_no_motion_bias = TRUE; @@ -1337,7 +1337,7 @@ inv_error_t inv_store_cal(unsigned char *calData, int length) } } - inv_get_array(INV_ACCEL_BIAS, bias); + inv_get_accel_bias(bias); /* write the accel biases */ for (i = 0; i < 3; i++) { diff --git a/libsensors/mlsdk/mllite/mlarray_legacy.c b/libsensors/mlsdk/mllite/mlarray_legacy.c deleted file mode 100644 index 20d9116..0000000 --- a/libsensors/mlsdk/mllite/mlarray_legacy.c +++ /dev/null @@ -1,587 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: mlarray_legacy.c $ - * - *****************************************************************************/ - -/** - * @defgroup MLArray_Legacy - * @brief Legacy Motion Library Array APIs. - * The Motion Library Array APIs provide the user access to the - * Motion Library state. These Legacy APIs provide access to - * individual state arrays using a data set name as the first - * argument to the API. This format has been replaced by unique - * named APIs for each data set, found in the MLArray group. - * - * @{ - * @file mlarray_legacy.c - * @brief The Legacy Motion Library Array APIs. - */ - -#include "ml.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "mlFIFO.h" -#include "mldl_cfg.h" - -/** - * @brief inv_get_array is used to get an array of processed motion sensor data. - * inv_get_array can be used to retrieve various data sets. Certain data - * sets require functions to be enabled using MLEnable in order to be - * valid. - * - * The available data sets are: - * - * - INV_ROTATION_MATRIX - * - INV_QUATERNION - * - INV_EULER_ANGLES_X - * - INV_EULER_ANGLES_Y - * - INV_EULER_ANGLES_Z - * - INV_EULER_ANGLES - * - INV_LINEAR_ACCELERATION - * - INV_LINEAR_ACCELERATION_WORLD - * - INV_GRAVITY - * - INV_ANGULAR_VELOCITY - * - INV_RAW_DATA - * - INV_GYROS - * - INV_ACCELS - * - INV_MAGNETOMETER - * - INV_GYRO_BIAS - * - INV_ACCEL_BIAS - * - INV_MAG_BIAS - * - INV_HEADING - * - INV_MAG_BIAS_ERROR - * - INV_PRESSURE - * - * Please refer to the documentation of inv_get_float_array() for a - * description of these data sets. - * - * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() - * must have been called. - * - * @param dataSet - * A constant specifying an array of data processed by the - * motion processor. - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_array(int dataSet, long *data) -{ - inv_error_t result; - switch (dataSet) { - case INV_GYROS: - result = inv_get_gyro(data); - break; - case INV_ACCELS: - result = inv_get_accel(data); - break; - case INV_TEMPERATURE: - result = inv_get_temperature(data); - break; - case INV_ROTATION_MATRIX: - result = inv_get_rot_mat(data); - break; - case INV_QUATERNION: - result = inv_get_quaternion(data); - break; - case INV_LINEAR_ACCELERATION: - result = inv_get_linear_accel(data); - break; - case INV_LINEAR_ACCELERATION_WORLD: - result = inv_get_linear_accel_in_world(data); - break; - case INV_GRAVITY: - result = inv_get_gravity(data); - break; - case INV_ANGULAR_VELOCITY: - result = inv_get_angular_velocity(data); - break; - case INV_EULER_ANGLES: - result = inv_get_euler_angles(data); - break; - case INV_EULER_ANGLES_X: - result = inv_get_euler_angles_x(data); - break; - case INV_EULER_ANGLES_Y: - result = inv_get_euler_angles_y(data); - break; - case INV_EULER_ANGLES_Z: - result = inv_get_euler_angles_z(data); - break; - case INV_GYRO_TEMP_SLOPE: - result = inv_get_gyro_temp_slope(data); - break; - case INV_GYRO_BIAS: - result = inv_get_gyro_bias(data); - break; - case INV_ACCEL_BIAS: - result = inv_get_accel_bias(data); - break; - case INV_MAG_BIAS: - result = inv_get_mag_bias(data); - break; - case INV_RAW_DATA: - result = inv_get_gyro_and_accel_sensor(data); - break; - case INV_MAG_RAW_DATA: - result = inv_get_mag_raw_data(data); - break; - case INV_MAGNETOMETER: - result = inv_get_magnetometer(data); - break; - case INV_PRESSURE: - result = inv_get_pressure(data); - break; - case INV_HEADING: - result = inv_get_heading(data); - break; - case INV_GYRO_CALIBRATION_MATRIX: - result = inv_get_gyro_cal_matrix(data); - break; - case INV_ACCEL_CALIBRATION_MATRIX: - result = inv_get_accel_cal_matrix(data); - break; - case INV_MAG_CALIBRATION_MATRIX: - result = inv_get_mag_cal_matrix(data); - break; - case INV_MAG_BIAS_ERROR: - result = inv_get_mag_bias_error(data); - break; - case INV_MAG_SCALE: - result = inv_get_mag_scale(data); - break; - case INV_LOCAL_FIELD: - result = inv_get_local_field(data); - break; - case INV_RELATIVE_QUATERNION: - result = inv_get_relative_quaternion(data); - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief inv_get_float_array is used to get an array of processed motion sensor - * data. inv_get_array can be used to retrieve various data sets. - * Certain data sets require functions to be enabled using MLEnable - * in order to be valid. - * - * The available data sets are: - * - * - INV_ROTATION_MATRIX : - * Returns an array of nine data points representing the rotation - * matrix generated from all available sensors. - * This requires that ML_SENSOR_FUSION be enabled. - * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, - * R33, representing the matrix: - * <center>R11 R12 R13</center> - * <center>R21 R22 R23</center> - * <center>R31 R32 R33</center> - * <b>Please refer to the "9-Axis Sensor Fusion Application Note" document, - * section 7 "Sensor Fusion Output", for details regarding rotation - * matrix output</b>. - * - * - INV_QUATERNION : - * Returns an array of four data points representing the quaternion - * generated from all available sensors. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_EULER_ANGLES_X : - * Returns an array of three data points representing roll, pitch, and - * yaw using the X axis of the gyroscope, accelerometer, and compass as - * reference axis. - * This is typically the convention used for mobile devices where the X - * axis is the width of the screen, Y axis is the height, and Z the - * depth. In this case roll is defined as the rotation around the X - * axis of the device. - * The euler angles convention for this output is the following: - * <TABLE> - * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> - * <TR><TD>roll </TD><TD>X axis </TD></TR> - * <TR><TD>pitch </TD><TD>Y axis </TD></TR> - * <TR><TD>yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * INV_EULER_ANGLES_X corresponds to the INV_EULER_ANGLES output and is - * therefore the default convention. - * - * - INV_EULER_ANGLES_Y : - * Returns an array of three data points representing roll, pitch, and - * yaw using the Y axis of the gyroscope, accelerometer, and compass as - * reference axis. - * This convention is typically used in augmented reality applications, - * where roll is defined as the rotation around the axis along the - * height of the screen of a mobile device, namely the Y axis. - * The euler angles convention for this output is the following: - * <TABLE> - * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> - * <TR><TD>roll </TD><TD>Y axis </TD></TR> - * <TR><TD>pitch </TD><TD>X axis </TD></TR> - * <TR><TD>yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * - * - INV_EULER_ANGLES_Z : - * Returns an array of three data points representing roll, pitch, and - * yaw using the Z axis of the gyroscope, accelerometer, and compass as - * reference axis. - * This convention is mostly used in application involving the use - * of a camera, typically placed on the back of a mobile device, that - * is along the Z axis. In this convention roll is defined as the - * rotation around the Z axis. - * The euler angles convention for this output is the following: - * <TABLE> - * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> - * <TR><TD>roll </TD><TD>Z axis </TD></TR> - * <TR><TD>pitch </TD><TD>X axis </TD></TR> - * <TR><TD>yaw </TD><TD>Y axis </TD></TR> - * </TABLE> - * - * - INV_EULER_ANGLES : - * Returns an array of three data points representing roll, pitch, and - * yaw corresponding to the INV_EULER_ANGLES_X output and it is - * therefore the default convention for Euler angles. - * Please refer to the INV_EULER_ANGLES_X for a detailed description. - * - * - INV_LINEAR_ACCELERATION : - * Returns an array of three data points representing the linear - * acceleration as derived from both gyroscopes and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_LINEAR_ACCELERATION_WORLD : - * Returns an array of three data points representing the linear - * acceleration in world coordinates, as derived from both gyroscopes - * and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_GRAVITY : - * Returns an array of three data points representing the direction - * of gravity in body coordinates, as derived from both gyroscopes - * and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_ANGULAR_VELOCITY : - * Returns an array of three data points representing the angular - * velocity as derived from <b>both</b> gyroscopes and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled, to fuse data from - * the gyroscope and accelerometer device, appropriately scaled and - * oriented according to the respective mounting matrices. - * - * - INV_RAW_DATA : - * Returns an array of nine data points representing raw sensor data - * of the gyroscope X, Y, Z, accelerometer X, Y, Z, and - * compass X, Y, Z values. - * These values are not scaled and come out directly from the devices' - * sensor data output. In case of accelerometers with lower output - * resolution, e.g 8-bit, the sensor data is scaled up to match the - * 2^14 = 1 gee typical representation for a +/- 2 gee full scale - * range. - * - * - INV_GYROS : - * Returns an array of three data points representing the X gyroscope, - * Y gyroscope, and Z gyroscope values. - * The values are not sensor fused with other sensor types data but - * reflect the orientation from the mounting matrices in use. - * The INV_GYROS values are scaled to ensure 1 dps corresponds to 2^16 - * codes. - * - * - INV_ACCELS : - * Returns an array of three data points representing the X - * accelerometer, Y accelerometer, and Z accelerometer values. - * The values are not sensor fused with other sensor types data but - * reflect the orientation from the mounting matrices in use. - * The INV_ACCELS values are scaled to ensure 1 gee corresponds to 2^16 - * codes. - * - * - INV_MAGNETOMETER : - * Returns an array of three data points representing the compass - * X, Y, and Z values. - * The values are not sensor fused with other sensor types data but - * reflect the orientation from the mounting matrices in use. - * The INV_MAGNETOMETER values are scaled to ensure 1 micro Tesla (uT) - * corresponds to 2^16 codes. - * - * - INV_GYRO_BIAS : - * Returns an array of three data points representing the gyroscope - * biases. - * - * - INV_ACCEL_BIAS : - * Returns an array of three data points representing the - * accelerometer biases. - * - * - INV_MAG_BIAS : - * Returns an array of three data points representing the compass - * biases. - * - * - INV_GYRO_CALIBRATION_MATRIX : - * Returns an array of nine data points representing the calibration - * matrix for the gyroscopes: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * - INV_ACCEL_CALIBRATION_MATRIX : - * Returns an array of nine data points representing the calibration - * matrix for the accelerometers: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * - INV_MAG_CALIBRATION_MATRIX : - * Returns an array of nine data points representing the calibration - * matrix for the compass: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * - INV_PRESSURE : - * Returns a single value representing the pressure in Pascal - * - * - INV_HEADING : - * Returns a single number representing the heading of the device - * relative to the Earth, in which 0 represents North, 90 degrees - * represents East, and so on. - * The heading is defined as the direction of the +Y axis if the Y - * axis is horizontal, and otherwise the direction of the -Z axis. - * - * - INV_MAG_BIAS_ERROR : - * Returns an array of three numbers representing the current estimated - * error in the compass biases. These numbers are unitless and serve - * as rough estimates in which numbers less than 100 typically represent - * reasonably well calibrated compass axes. - * - * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() - * must have been called. - * - * @param dataSet - * A constant specifying an array of data processed by - * the motion processor. - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_float_array(int dataSet, float *data) -{ - inv_error_t result; - switch (dataSet) { - case INV_GYROS: - result = inv_get_gyro_float(data); - break; - case INV_ACCELS: - result = inv_get_accel_float(data); - break; - case INV_TEMPERATURE: - result = inv_get_temperature_float(data); - break; - case INV_ROTATION_MATRIX: - result = inv_get_rot_mat_float(data); - break; - case INV_QUATERNION: - result = inv_get_quaternion_float(data); - break; - case INV_LINEAR_ACCELERATION: - result = inv_get_linear_accel_float(data); - break; - case INV_LINEAR_ACCELERATION_WORLD: - result = inv_get_linear_accel_in_world_float(data); - break; - case INV_GRAVITY: - result = inv_get_gravity_float(data); - break; - case INV_ANGULAR_VELOCITY: - result = inv_get_angular_velocity_float(data); - break; - case INV_EULER_ANGLES: - result = inv_get_euler_angles_float(data); - break; - case INV_EULER_ANGLES_X: - result = inv_get_euler_angles_x_float(data); - break; - case INV_EULER_ANGLES_Y: - result = inv_get_euler_angles_y_float(data); - break; - case INV_EULER_ANGLES_Z: - result = inv_get_euler_angles_z_float(data); - break; - case INV_GYRO_TEMP_SLOPE: - result = inv_get_gyro_temp_slope_float(data); - break; - case INV_GYRO_BIAS: - result = inv_get_gyro_bias_float(data); - break; - case INV_ACCEL_BIAS: - result = inv_get_accel_bias_float(data); - break; - case INV_MAG_BIAS: - result = inv_get_mag_bias_float(data); - break; - case INV_RAW_DATA: - result = inv_get_gyro_and_accel_sensor_float(data); - break; - case INV_MAG_RAW_DATA: - result = inv_get_mag_raw_data_float(data); - break; - case INV_MAGNETOMETER: - result = inv_get_magnetometer_float(data); - break; - case INV_PRESSURE: - result = inv_get_pressure_float(data); - break; - case INV_HEADING: - result = inv_get_heading_float(data); - break; - case INV_GYRO_CALIBRATION_MATRIX: - result = inv_get_gyro_cal_matrix_float(data); - break; - case INV_ACCEL_CALIBRATION_MATRIX: - result = inv_get_accel_cal_matrix_float(data); - break; - case INV_MAG_CALIBRATION_MATRIX: - result = inv_get_mag_cal_matrix_float(data); - break; - case INV_MAG_BIAS_ERROR: - result = inv_get_mag_bias_error_float(data); - break; - case INV_MAG_SCALE: - result = inv_get_mag_scale_float(data); - break; - case INV_LOCAL_FIELD: - result = inv_get_local_field_float(data); - break; - case INV_RELATIVE_QUATERNION: - result = inv_get_relative_quaternion_float(data); - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief used to set an array of motion sensor data. - * Handles the following data sets: - * - INV_GYRO_BIAS - * - INV_ACCEL_BIAS - * - INV_MAG_BIAS - * - INV_GYRO_TEMP_SLOPE - * - * For more details about the use of the data sets - * please refer to the documentation of inv_set_float_array(). - * - * Please also refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() or - * MLDmpPedometerStandAloneOpen() - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param dataSet A constant specifying an array of data. - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_array(int dataSet, long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - switch (dataSet) { - case INV_GYRO_BIAS: - result = inv_set_gyro_bias(data); - break; - case INV_ACCEL_BIAS: - result = inv_set_accel_bias(data); - break; - case INV_MAG_BIAS: - result = inv_set_mag_bias(data); - break; - case INV_GYRO_TEMP_SLOPE: - result = inv_set_gyro_temp_slope(data); - break; - case INV_LOCAL_FIELD: - result = inv_set_local_field(data); - break; - case INV_MAG_SCALE: - result = inv_set_mag_scale(data); - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief used to set an array of motion sensor data. - * Handles various data sets: - * - INV_GYRO_BIAS - * - INV_ACCEL_BIAS - * - INV_MAG_BIAS - * - INV_GYRO_TEMP_SLOPE - * - * Please refer to the provided "9-Axis Sensor Fusion Application - * Note" document provided. - * - * @pre MLDmpOpen() or - * MLDmpPedometerStandAloneOpen() - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param dataSet A constant specifying an array of data. - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_float_array(int dataSet, float *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - switch (dataSet) { - case INV_GYRO_TEMP_SLOPE: // internal - result = inv_set_gyro_temp_slope_float(data); - break; - case INV_GYRO_BIAS: // internal - result = inv_set_gyro_bias_float(data); - break; - case INV_ACCEL_BIAS: // internal - result = inv_set_accel_bias_float(data); - break; - case INV_MAG_BIAS: // internal - result = inv_set_mag_bias_float(data); - break; - case INV_LOCAL_FIELD: // internal - result = inv_set_local_field_float(data); - break; - case INV_MAG_SCALE: // internal - result = inv_set_mag_scale_float(data); - break; - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} diff --git a/libsensors/mlsdk/mllite/mlcompat.c b/libsensors/mlsdk/mllite/mlcompat.c new file mode 100644 index 0000000..1fc9f0f --- /dev/null +++ b/libsensors/mlsdk/mllite/mlcompat.c @@ -0,0 +1,315 @@ +/** + * mlcompat! where symbols referenced by libinvensense_mpl.so go to die. :( + * They may one day rise again, e.g. inv_get_motion_state looks fun, but + * this helps keep track of where things are used and by whom are they used. + */ +#include "mlcompat.h" +#include "ml.h" +#include "mlMathFunc.h" +#include "mlmath.h" + +inv_error_t inv_pressure_supervisor(void) +{ + return INV_SUCCESS; +} + + + +/** + * @brief inv_get_motion_state is used to determine if the device is in + * a 'motion' or 'no motion' state. + * inv_get_motion_state returns INV_MOTION of the device is moving, + * or INV_NO_MOTION if the device is not moving. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must have been called. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +int inv_get_motion_state(void) +{ + return inv_obj.motion_state; +} + + + +/** + * Performs one iteration of the filter, generating a new y(0) + * 1 / N / N \\ + * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length + * a(0) \k=0 \ k=1 // + * + * The filters A and B should be (sizeof(long) * state->length). + * The state variables x and y should be (sizeof(long) * (state->length - 1)) + * + * The state variables x and y should be initialized prior to the first call + * + * @param state Contains the length of the filter, filter coefficients and + * filter state variables x and y. + * @param x New input into the filter. + */ +void inv_filter_long(struct filter_long *state, long x) +{ + const long *b = state->b; + const long *a = state->a; + long length = state->length; + long long tmp; + int ii; + + /* filter */ + tmp = (long long)x *(b[0]); + for (ii = 0; ii < length - 1; ii++) { + tmp += ((long long)state->x[ii] * (long long)(b[ii + 1])); + } + for (ii = 0; ii < length - 1; ii++) { + tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1])); + } + tmp /= (long long)a[0]; + + /* Delay */ + for (ii = length - 2; ii > 0; ii--) { + state->y[ii] = state->y[ii - 1]; + state->x[ii] = state->x[ii - 1]; + } + /* New values */ + state->y[0] = (long)tmp; + state->x[0] = x; +} + +void inv_q_multf(const float *q1, const float *q2, float *qProd) +{ + 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(float *q1, float *q2, float *qSum) +{ + 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) +{ + 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] 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) +{ + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +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 + 10 * k + l) = *(a + 10 * i + j); + } + } + *n = *n - 1; +} + +double inv_matrix_detd(double *p, int *n) +{ + double d[10][10], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 11) - *(p + 1) ** (p + 10)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_incd(p, &d[0][0], n, i, j); + sum = + sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0], + n); + } + + return (sum); +} + + + +inv_error_t inv_get_array(int dataSet, long *data) +{ + switch (dataSet) { + case INV_GYROS: + return inv_get_gyro(data); + case INV_ACCELS: + return inv_get_accel(data); + case INV_TEMPERATURE: + return inv_get_temperature(data); + case INV_ROTATION_MATRIX: + return inv_get_rot_mat(data); + case INV_QUATERNION: + return inv_get_quaternion(data); + case INV_LINEAR_ACCELERATION: + return inv_get_linear_accel(data); + case INV_LINEAR_ACCELERATION_WORLD: + return inv_get_linear_accel_in_world(data); + case INV_GRAVITY: + return inv_get_gravity(data); + case INV_ANGULAR_VELOCITY: + return inv_get_angular_velocity(data); + case INV_EULER_ANGLES: + return inv_get_euler_angles(data); + case INV_EULER_ANGLES_X: + return inv_get_euler_angles_x(data); + case INV_EULER_ANGLES_Y: + return inv_get_euler_angles_y(data); + case INV_EULER_ANGLES_Z: + return inv_get_euler_angles_z(data); + case INV_GYRO_TEMP_SLOPE: + return inv_get_gyro_temp_slope(data); + case INV_GYRO_BIAS: + return inv_get_gyro_bias(data); + case INV_ACCEL_BIAS: + return inv_get_accel_bias(data); + case INV_MAG_BIAS: + return inv_get_mag_bias(data); + case INV_RAW_DATA: + return inv_get_gyro_and_accel_sensor(data); + case INV_MAG_RAW_DATA: + return inv_get_mag_raw_data(data); + case INV_MAGNETOMETER: + return inv_get_magnetometer(data); + case INV_PRESSURE: + return inv_get_pressure(data); + case INV_HEADING: + return inv_get_heading(data); + case INV_GYRO_CALIBRATION_MATRIX: + return inv_get_gyro_cal_matrix(data); + case INV_ACCEL_CALIBRATION_MATRIX: + return inv_get_accel_cal_matrix(data); + case INV_MAG_CALIBRATION_MATRIX: + return inv_get_mag_cal_matrix(data); + case INV_MAG_BIAS_ERROR: + return inv_get_mag_bias_error(data); + case INV_MAG_SCALE: + return inv_get_mag_scale(data); + case INV_LOCAL_FIELD: + return inv_get_local_field(data); + case INV_RELATIVE_QUATERNION: + return inv_get_relative_quaternion(data); + } + return INV_ERROR_INVALID_PARAMETER; +} + +inv_error_t inv_get_float_array(int dataSet, float *data) +{ + switch (dataSet) { + case INV_GYROS: + return inv_get_gyro_float(data); + case INV_ACCELS: + return inv_get_accel_float(data); + case INV_TEMPERATURE: + return inv_get_temperature_float(data); + case INV_ROTATION_MATRIX: + return inv_get_rot_mat_float(data); + case INV_QUATERNION: + return inv_get_quaternion_float(data); + case INV_LINEAR_ACCELERATION: + return inv_get_linear_accel_float(data); + case INV_LINEAR_ACCELERATION_WORLD: + return inv_get_linear_accel_in_world_float(data); + case INV_GRAVITY: + return inv_get_gravity_float(data); + case INV_ANGULAR_VELOCITY: + return inv_get_angular_velocity_float(data); + case INV_EULER_ANGLES: + return inv_get_euler_angles_float(data); + case INV_EULER_ANGLES_X: + return inv_get_euler_angles_x_float(data); + case INV_EULER_ANGLES_Y: + return inv_get_euler_angles_y_float(data); + case INV_EULER_ANGLES_Z: + return inv_get_euler_angles_z_float(data); + case INV_GYRO_TEMP_SLOPE: + return inv_get_gyro_temp_slope_float(data); + case INV_GYRO_BIAS: + return inv_get_gyro_bias_float(data); + case INV_ACCEL_BIAS: + return inv_get_accel_bias_float(data); + case INV_MAG_BIAS: + return inv_get_mag_bias_float(data); + case INV_RAW_DATA: + return inv_get_gyro_and_accel_sensor_float(data); + case INV_MAG_RAW_DATA: + return inv_get_mag_raw_data_float(data); + case INV_MAGNETOMETER: + return inv_get_magnetometer_float(data); + case INV_PRESSURE: + return inv_get_pressure_float(data); + case INV_HEADING: + return inv_get_heading_float(data); + case INV_GYRO_CALIBRATION_MATRIX: + return inv_get_gyro_cal_matrix_float(data); + case INV_ACCEL_CALIBRATION_MATRIX: + return inv_get_accel_cal_matrix_float(data); + case INV_MAG_CALIBRATION_MATRIX: + return inv_get_mag_cal_matrix_float(data); + case INV_MAG_BIAS_ERROR: + return inv_get_mag_bias_error_float(data); + case INV_MAG_SCALE: + return inv_get_mag_scale_float(data); + case INV_LOCAL_FIELD: + return inv_get_local_field_float(data); + case INV_RELATIVE_QUATERNION: + return inv_get_relative_quaternion_float(data); + } + return INV_ERROR_INVALID_PARAMETER; +} diff --git a/libsensors/mlsdk/mllite/mlcompat.h b/libsensors/mlsdk/mllite/mlcompat.h new file mode 100644 index 0000000..860ab1e --- /dev/null +++ b/libsensors/mlsdk/mllite/mlcompat.h @@ -0,0 +1,69 @@ +#ifndef __INV_COMPAT_H__ +#define __INV_COMPAT_H__ + +#include "mltypes.h" + +#define INV_GYROS 0x0001 +#define INV_ACCELS 0x0002 +#define INV_ROTATION_MATRIX 0x0003 +#define INV_QUATERNION 0x0004 +#define INV_EULER_ANGLES 0x0005 +#define INV_LINEAR_ACCELERATION 0x0006 +#define INV_LINEAR_ACCELERATION_WORLD 0x0007 +#define INV_GRAVITY 0x0008 +#define INV_ANGULAR_VELOCITY 0x0009 +#define INV_GYRO_CALIBRATION_MATRIX 0x000B +#define INV_ACCEL_CALIBRATION_MATRIX 0x000C +#define INV_GYRO_BIAS 0x000D +#define INV_ACCEL_BIAS 0x000E +#define INV_GYRO_TEMP_SLOPE 0x000F +#define INV_RAW_DATA 0x0011 +#define INV_EULER_ANGLES_X 0x0013 +#define INV_EULER_ANGLES_Y 0x0014 +#define INV_EULER_ANGLES_Z 0x0015 +#define INV_MAGNETOMETER 0x001A +#define INV_MAG_RAW_DATA 0x001C +#define INV_MAG_CALIBRATION_MATRIX 0x001D +#define INV_MAG_BIAS 0x001E +#define INV_HEADING 0x001F +#define INV_MAG_BIAS_ERROR 0x0020 +#define INV_PRESSURE 0x0021 +#define INV_LOCAL_FIELD 0x0022 +#define INV_MAG_SCALE 0x0023 +#define INV_RELATIVE_QUATERNION 0x0024 +#define INV_TEMPERATURE 0x2000 + + +#ifdef __cplusplus +extern "C" { +#endif + + struct filter_long { + int length; + const long *b; + const long *a; + long *x; + long *y; + }; + + inv_error_t inv_pressure_supervisor(void); + + int inv_get_motion_state(void); + + void inv_filter_long(struct filter_long *state, long x); + void inv_q_multf(const float *q1, const float *q2, float *qProd); + void inv_q_addf(float *q1, 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_matrix_det_incd(double *a, double *b, int *n, int x, int y); + double inv_matrix_detd(double *p, int *n); + + inv_error_t inv_get_array(int dataSet, long *data); + inv_error_t inv_get_float_array(int dataSet, float *data); + +#ifdef __cplusplus +} +#endif + +#endif /* __INV_COMPAT_H__ */
\ No newline at end of file diff --git a/libsensors/mlsdk/mllite/mlcontrol.c b/libsensors/mlsdk/mllite/mlcontrol.c index 278438a..78f8e05 100644 --- a/libsensors/mlsdk/mllite/mlcontrol.c +++ b/libsensors/mlsdk/mllite/mlcontrol.c @@ -94,545 +94,6 @@ extern const unsigned char *dmpConfig1; /* -------------- */ /** - * @brief inv_set_control_sensitivity is used to set the sensitivity for a control - * signal. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param sensitivity The sensitivity of the control signal. - * - * @return error code - */ -inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, - long sensitivity) -{ - INVENSENSE_FUNC_START; - unsigned char regs[2]; - long finalSens = 0; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - finalSens = sensitivity * 100; - if (finalSens > 16384) { - finalSens = 16384; - } - regs[0] = (unsigned char)(finalSens / 256); - regs[1] = (unsigned char)(finalSens % 256); - switch (controlSignal) { - case INV_CONTROL_1: - result = inv_set_mpu_memory(KEY_D_0_224, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[0] = (unsigned short)sensitivity; - break; - case INV_CONTROL_2: - result = inv_set_mpu_memory(KEY_D_0_228, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[1] = (unsigned short)sensitivity; - break; - case INV_CONTROL_3: - result = inv_set_mpu_memory(KEY_D_0_232, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[2] = (unsigned short)sensitivity; - break; - case INV_CONTROL_4: - result = inv_set_mpu_memory(KEY_D_0_236, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[3] = (unsigned short)sensitivity; - break; - default: - break; - } - if (finalSens != sensitivity * 100) { - return INV_ERROR_INVALID_PARAMETER; - } else { - return INV_SUCCESS; - } -} - -/** - * @brief inv_set_control_func allows the user to choose how the sensor data will - * be processed in order to provide a control parameter. - * inv_set_control_func allows the user to choose which control functions - * will be incorporated in the sensor data processing. - * The control functions are: - * - INV_GRID - * Indicates that the user will be controlling a system that - * has discrete steps, such as icons, menu entries, pixels, etc. - * - INV_SMOOTH - * Indicates that noise from unintentional motion should be filtered out. - * - INV_DEAD_ZONE - * Indicates that a dead zone should be used, below which sensor - * data is set to zero. - * - INV_HYSTERESIS - * Indicates that, when INV_GRID is selected, hysteresis should - * be used to prevent the control signal from switching rapidly across - * elements of the grid. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param function Indicates what functions will be used. - * Can be a bitwise OR of several values. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_set_control_func(unsigned short function) -{ - INVENSENSE_FUNC_START; - unsigned char regs[8] = { DINA06, DINA26, - DINA46, DINA66, - DINA0E, DINA2E, - DINA4E, DINA6E - }; - unsigned char i; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if ((function & INV_SMOOTH) == 0) { - for (i = 0; i < 8; i++) { - regs[i] = DINA80 + 3; - } - } - result = inv_set_mpu_memory(KEY_CFG_4, 8, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.functions = function; - result = inv_set_dead_zone(); - - return result; -} - -/** - * @brief inv_get_control_signal is used to get the current control signal with - * high precision. - * inv_get_control_signal is used to acquire the current data of a control signal. - * If INV_GRID is being used, inv_get_grid_number will probably be preferrable. - * - * @param controlSignal Indicates which control signal is being queried. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param reset Indicates whether the control signal should be reset to zero. - * Options are INV_RESET or INV_NO_RESET - * @param data A pointer to the current control signal data. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_control_signal(unsigned short controlSignal, - unsigned short reset, long *data) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - *data = cntrl_obj.controlInt[0]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[0] = 0; - } - break; - case INV_CONTROL_2: - *data = cntrl_obj.controlInt[1]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[1] = 0; - } - break; - case INV_CONTROL_3: - *data = cntrl_obj.controlInt[2]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[2] = 0; - } - break; - case INV_CONTROL_4: - *data = cntrl_obj.controlInt[3]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[3] = 0; - } - break; - default: - break; - } - return INV_SUCCESS; -} - -/** - * @brief inv_get_grid_num is used to get the current grid location for a certain - * control signal. - * inv_get_grid_num is used to acquire the current grid location. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being queried. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param reset Indicates whether the control signal should be reset to zero. - * Options are INV_RESET or INV_NO_RESET - * @param data A pointer to the current grid number. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset, - long *data) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - *data = cntrl_obj.gridNum[0]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[0] = 0; - } - break; - case INV_CONTROL_2: - *data = cntrl_obj.gridNum[1]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[1] = 0; - } - break; - case INV_CONTROL_3: - *data = cntrl_obj.gridNum[2]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[2] = 0; - } - break; - case INV_CONTROL_4: - *data = cntrl_obj.gridNum[3]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[3] = 0; - } - break; - default: - break; - } - - return INV_SUCCESS; -} - -/** - * @brief inv_set_grid_thresh is used to set the grid size for a control signal. - * inv_set_grid_thresh is used to adjust the size of the grid being controlled. - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 and - * - INV_CONTROL_4. - * @param threshold The threshold of the control signal at which the grid - * number will be incremented or decremented. - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - cntrl_params.gridThreshold[0] = threshold; - break; - case INV_CONTROL_2: - cntrl_params.gridThreshold[1] = threshold; - break; - case INV_CONTROL_3: - cntrl_params.gridThreshold[2] = threshold; - break; - case INV_CONTROL_4: - cntrl_params.gridThreshold[3] = threshold; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - - return INV_SUCCESS; -} - -/** - * @brief inv_set_grid_max is used to set the maximum grid number for a control signal. - * inv_set_grid_max is used to adjust the maximum allowed grid number, above - * which the grid number will not be incremented. - * The minimum grid number is always zero. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 and - * - INV_CONTROL_4. - * - * @param maximum The maximum grid number for a control signal. - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - cntrl_params.gridMaximum[0] = maximum; - break; - case INV_CONTROL_2: - cntrl_params.gridMaximum[1] = maximum; - break; - case INV_CONTROL_3: - cntrl_params.gridMaximum[2] = maximum; - break; - case INV_CONTROL_4: - cntrl_params.gridMaximum[3] = maximum; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - - return INV_SUCCESS; -} - -/** - * @brief GridCallback function pointer type, to be passed as argument of - * inv_set_grid_callback. - * - * @param controlSignal Indicates which control signal crossed a grid threshold. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 and - * - INV_CONTROL_4. - * - * @param gridNumber An array of four numbers representing the grid number for each - * control signal. - * @param gridChange An array of four numbers representing the change in grid number - * for each control signal. -**/ -typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum, - long *gridChange); - -/** - * @brief inv_set_grid_callback is used to register a callback function that - * will trigger when the grid location changes. - * inv_set_grid_callback allows a user to define a callback function that will - * run when a control signal crosses a grid threshold. - - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). inv_dmp_start must <b>NOT</b> have - * been called. - * - * @param func A user defined callback function - * @return Zero if the command is successful; an ML error code otherwise. -**/ -inv_error_t inv_set_grid_callback(fpGridCb func) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - cntrl_params.gridCallback = func; - return INV_SUCCESS; -} - -/** - * @brief inv_set_control_data is used to assign physical parameters to control signals. - * inv_set_control_data allows flexibility in assigning physical parameters to - * control signals. For example, the user is allowed to use raw gyroscope data - * as an input to the control algorithm. - * Alternatively, angular velocity can be used, which combines gyroscopes and - * accelerometers to provide a more robust physical parameter. Finally, angular - * velocity in world coordinates can be used, providing a control signal in - * which pitch and yaw are provided relative to gravity. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param parameterArray Indicates which parameter array is being assigned to a - * control signal. Must be one of: - * - INV_GYROS, - * - INV_ANGULAR_VELOCITY, or - * - * @param parameterAxis Indicates which axis of the parameter array will be used. - * Must be: - * - INV_ROLL, - * - INV_PITCH, or - * - INV_YAW. - */ - -inv_error_t inv_set_control_data(unsigned short controlSignal, - unsigned short parameterArray, - unsigned short parameterAxis) -{ - INVENSENSE_FUNC_START; - unsigned char regs[2] = { DINA80 + 10, DINA20 }; - inv_error_t result; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (parameterArray == INV_ANGULAR_VELOCITY) { - regs[0] = DINA80 + 5; - regs[1] = DINA00; - } - switch (controlSignal) { - case INV_CONTROL_1: - cntrl_params.parameterArray[0] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += 0x02; - cntrl_params.parameterAxis[0] = 0; - break; - case INV_ROLL: - regs[1] = DINA22; - cntrl_params.parameterAxis[0] = 1; - break; - case INV_YAW: - regs[1] = DINA42; - cntrl_params.parameterAxis[0] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case INV_CONTROL_2: - cntrl_params.parameterArray[1] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += DINA0E; - cntrl_params.parameterAxis[1] = 0; - break; - case INV_ROLL: - regs[1] += DINA2E; - cntrl_params.parameterAxis[1] = 1; - break; - case INV_YAW: - regs[1] += DINA4E; - cntrl_params.parameterAxis[1] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case INV_CONTROL_3: - cntrl_params.parameterArray[2] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += DINA0E; - cntrl_params.parameterAxis[2] = 0; - break; - case INV_ROLL: - regs[1] += DINA2E; - cntrl_params.parameterAxis[2] = 1; - break; - case INV_YAW: - regs[1] += DINA4E; - cntrl_params.parameterAxis[2] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case INV_CONTROL_4: - cntrl_params.parameterArray[3] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += DINA0E; - cntrl_params.parameterAxis[3] = 0; - break; - case INV_ROLL: - regs[1] += DINA2E; - cntrl_params.parameterAxis[3] = 1; - break; - case INV_YAW: - regs[1] += DINA4E; - cntrl_params.parameterAxis[3] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** * @brief inv_get_control_data is used to get the current control data. * * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or @@ -650,10 +111,15 @@ inv_error_t inv_set_control_data(unsigned short controlSignal, * * @return Zero if the command is successful; an ML error code otherwise. */ - inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, long *gridChange) { + /* NOTE: + * This symbol is referenced by libinvensense_mpl.so + * That is the sole reason this is being kept around. + * It's integrated with some other junk here though, + * so for the moment I'm not comfortable moving it. + */ INVENSENSE_FUNC_START; int_fast8_t i = 0; @@ -669,129 +135,5 @@ inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, } /** - * @internal - * @brief Update the ML Control engine. This function should be called - * every time new data from the MPU becomes available. - * Control engine outputs are written to the cntrl_obj data - * structure. - * @return INV_SUCCESS or an error code. -**/ -inv_error_t inv_update_control(struct inv_obj_t * inv_obj __unused) -{ - INVENSENSE_FUNC_START; - unsigned char i; - long gridTmp; - long tmp; - - inv_get_cntrl_data(cntrl_obj.mlGridNumDMP); - - for (i = 0; i < 4; i++) { - if (cntrl_params.functions & INV_GRID) { - if (cntrl_params.functions & INV_HYSTERESIS) { - cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i]; - } - cntrl_obj.mlGridNumDMP[i] = - cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L; - cntrl_obj.controlInt[i] = - (cntrl_obj.mlGridNumDMP[i] % - (128 * cntrl_params.gridThreshold[i])) / 128; - gridTmp = - cntrl_obj.mlGridNumDMP[i] / (128 * - cntrl_params.gridThreshold[i]); - tmp = 1 + 16777216L / cntrl_params.gridThreshold[i]; - cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i]; - if (cntrl_obj.gridChange[i] > tmp / 2) { - cntrl_obj.gridChange[i] = - gridTmp - tmp - cntrl_obj.lastGridNum[i]; - } else if (cntrl_obj.gridChange[i] < -tmp / 2) { - cntrl_obj.gridChange[i] = - gridTmp + tmp - cntrl_obj.lastGridNum[i]; - } - if ((cntrl_params.functions & INV_HYSTERESIS) - && (cntrl_obj.gridChange[i] != 0)) { - if (cntrl_obj.gridChange[i] > 0) { - cntrl_obj.gridNumOffset[i] += - 128 * cntrl_params.gridThreshold[i]; - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - if (cntrl_obj.gridChange[i] < 0) { - cntrl_obj.gridNumOffset[i] -= - 128 * cntrl_params.gridThreshold[i]; - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - } - cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i]; - if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) { - cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i]; - if (cntrl_obj.controlInt[i] >= - cntrl_params.gridThreshold[i] / 2) { - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - } else if (cntrl_obj.gridNum[i] <= 0) { - cntrl_obj.gridNum[i] = 0; - if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) { - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - } - cntrl_obj.lastGridNum[i] = gridTmp; - if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) { - cntrl_params.gridCallback((INV_CONTROL_1 << i), - cntrl_obj.gridNum, - cntrl_obj.gridChange); - } - - } else { - cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i]; - } - - } - - return INV_SUCCESS; -} - -/** - * @brief Enables the INV_CONTROL engine. - * - * @note This function replaces MLEnable(INV_CONTROL) - * - * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must - * have been called. - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_enable_control(void) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - memset(&cntrl_obj, 0, sizeof(cntrl_obj)); - - inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo - return INV_SUCCESS; -} - -/** - * @brief Disables the INV_CONTROL engine. - * - * @note This function replaces MLDisable(INV_CONTROL) - * - * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must - * have been called. - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_disable_control(void) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - return INV_SUCCESS; -} - -/** * @} */ diff --git a/libsensors/mlsdk/mllite/mlcontrol.h b/libsensors/mlsdk/mllite/mlcontrol.h index abdefa3..d23aa2b 100644 --- a/libsensors/mlsdk/mllite/mlcontrol.h +++ b/libsensors/mlsdk/mllite/mlcontrol.h @@ -46,9 +46,6 @@ extern "C" { #include "mltypes.h" #include "ml.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlcontrol_legacy.h" -#endif /* ------------ */ /* - Defines. - */ @@ -184,32 +181,9 @@ extern "C" { /* ML Control Functions. */ /**************************************************************************/ - unsigned short inv_get_control_params(struct control_params *params); - unsigned short inv_set_control_params(struct control_params *params); - /*API for handling control signals */ - inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, - long sensitivity); - inv_error_t inv_set_control_func(unsigned short function); - inv_error_t inv_get_control_signal(unsigned short controlSignal, - unsigned short reset, long *data); - inv_error_t inv_get_grid_num(unsigned short controlSignal, - unsigned short reset, long *data); - inv_error_t inv_set_grid_thresh(unsigned short controlSignal, - long threshold); - inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum); - inv_error_t - inv_set_grid_callback(void (*func) - (unsigned short controlSignal, long *gridNum, - long *gridChange)); - inv_error_t inv_set_control_data(unsigned short controlSignal, - unsigned short parameterArray, - unsigned short parameterNum); inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, long *gridChange); - inv_error_t inv_update_control(struct inv_obj_t *inv_obj); - inv_error_t inv_enable_control(void); - inv_error_t inv_disable_control(void); #ifdef __cplusplus } diff --git a/libsensors/mlsdk/mllite/mldl.c b/libsensors/mlsdk/mllite/mldl.c index ee7258f..ca6de8f 100644 --- a/libsensors/mlsdk/mllite/mldl.c +++ b/libsensors/mlsdk/mllite/mldl.c @@ -50,7 +50,6 @@ #include "dmpKey.h" #include "mlFIFOHW.h" #include "compass.h" -#include "pressure.h" #include "log.h" #undef MPL_LOG_TAG @@ -686,7 +685,7 @@ inv_error_t inv_set_offset(const short *offset) * @return zero if the command is successful, an error code otherwise. * @endif */ -inv_error_t +static inv_error_t inv_get_mpu_memory_one_bank(unsigned char bank, unsigned char memAddr, unsigned short length, unsigned char *buffer) @@ -732,6 +731,7 @@ inv_get_mpu_memory_one_bank(unsigned char bank, * @return zero if the command is successful, an error code otherwise. * @endif */ +static inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, unsigned short memAddr, unsigned short length, @@ -954,42 +954,6 @@ unsigned char inv_get_product_rev(void) ******************************************************************************/ /** - * @brief inv_get_interrupt_status returns the interrupt status from the specified - * interrupt pin. - * @param intPin - * Currently only the value INTPIN_MPU is supported. - * @param status - * The available statuses are: - * - BIT_MPU_RDY_EN - * - BIT_DMP_INT_EN - * - BIT_RAW_RDY_EN - * - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_interrupt_status(unsigned char intPin, - unsigned char *status) -{ - INVENSENSE_FUNC_START; - - inv_error_t result; - - switch (intPin) { - - case INTPIN_MPU: - /*---- return the MPU interrupt status ----*/ - result = inv_serial_read(sMLSLHandle, mldlCfg.addr, - MPUREG_INT_STATUS, 1, status); - break; - - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} - -/** * @brief query the current status of an interrupt source. * @param srcIndex * index of the interrupt source. @@ -1015,37 +979,6 @@ void inv_clear_interrupt_trigger(unsigned char srcIndex) intTrigger[srcIndex] = 0; } -/** - * @brief inv_interrupt_handler function should be called when an interrupt is - * received. The source parameter identifies which interrupt source - * caused the interrupt. Note that this routine should not be called - * directly from the interrupt service routine. - * - * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1, - * INTSRC_AUX2, or INT_SRC_TIMER. - * - * @return Zero if the command is successful; an error code otherwise. - */ -inv_error_t inv_interrupt_handler(unsigned char intSource) -{ - INVENSENSE_FUNC_START; - /*---- range check ----*/ - if (intSource >= NUM_OF_INTSOURCES) { - return INV_ERROR; - } - - /*---- save source of interrupt ----*/ - intTrigger[intSource] = INT_TRIGGERED; - -#ifdef ML_USE_DMP_SIM - if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { - MLSimHWDataInput(); - } -#endif - - return INV_SUCCESS; -} - /***************************/ /**@}*//* end of defgroup */ /***************************/ diff --git a/libsensors/mlsdk/mllite/mldl.h b/libsensors/mlsdk/mllite/mldl.h index 961d860..b4a8616 100644 --- a/libsensors/mlsdk/mllite/mldl.h +++ b/libsensors/mlsdk/mllite/mldl.h @@ -28,9 +28,6 @@ #include "mlsl.h" #include <linux/mpu.h> #include "mldl_cfg.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mldl_legacy.h" -#endif /* ------------ */ /* - Defines. - */ @@ -155,21 +152,9 @@ extern "C" { unsigned short inv_dl_get_address(unsigned short key); uint_fast8_t inv_dmpkey_supported(unsigned short key); - inv_error_t inv_get_interrupt_status(unsigned char intPin, - unsigned char *value); unsigned char inv_get_interrupt_trigger(unsigned char index); void inv_clear_interrupt_trigger(unsigned char index); - inv_error_t inv_interrupt_handler(unsigned char intSource); - /** Only exposed for testing purposes */ - inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, - unsigned short memAddr, - unsigned short length, - const unsigned char *buffer); - inv_error_t inv_get_mpu_memory_one_bank(unsigned char bank, - unsigned char memAddr, - unsigned short length, - unsigned char *buffer); #ifdef __cplusplus } #endif diff --git a/libsensors/mlsdk/mllite/mldl_cfg.h b/libsensors/mlsdk/mllite/mldl_cfg.h index fb2e402..90252c5 100644 --- a/libsensors/mlsdk/mllite/mldl_cfg.h +++ b/libsensors/mlsdk/mllite/mldl_cfg.h @@ -55,7 +55,6 @@ #define INV_Y_PRESSURE (0x0800) #define INV_Z_PRESSURE (0x1000) -#define INV_TEMPERATURE (0x2000) #define INV_TIME (0x4000) #define INV_THREE_AXIS_GYRO (0x000F) diff --git a/libsensors/mlsdk/mllite/mldmp.h b/libsensors/mlsdk/mllite/mldmp.h index ff3d24e..1b57bef 100644 --- a/libsensors/mlsdk/mllite/mldmp.h +++ b/libsensors/mlsdk/mllite/mldmp.h @@ -74,9 +74,6 @@ #ifndef MLDMP_H #define MLDMP_H -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mldmp_legacy.h" -#endif #ifdef __cplusplus extern "C" { diff --git a/libsensors/mlsdk/mllite/mlstates.c b/libsensors/mlsdk/mllite/mlstates.c index e44f100..260e9d2 100644 --- a/libsensors/mlsdk/mllite/mlstates.c +++ b/libsensors/mlsdk/mllite/mlstates.c @@ -85,21 +85,17 @@ static inv_error_t MLStateCloseCallbacks(void) * @param state The state of which the label has to be returned. * @return A string containing the state label. **/ -char *inv_state_name(unsigned char state) +static char *inv_state_name(unsigned char state) { switch (state) { case INV_STATE_SERIAL_CLOSED: return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED); - break; case INV_STATE_SERIAL_OPENED: return INV_STATE_NAME(INV_STATE_SERIAL_OPENED); - break; case INV_STATE_DMP_OPENED: return INV_STATE_NAME(INV_STATE_DMP_OPENED); - break; case INV_STATE_DMP_STARTED: return INV_STATE_NAME(INV_STATE_DMP_STARTED); - break; default: return NULL; } diff --git a/libsensors/mlsdk/mllite/mlstates.h b/libsensors/mlsdk/mllite/mlstates.h index cbaa610..b617b46 100644 --- a/libsensors/mlsdk/mllite/mlstates.h +++ b/libsensors/mlsdk/mllite/mlstates.h @@ -25,9 +25,6 @@ #define INV_STATES_H__ #include "mltypes.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlstates_legacy.h" -#endif #ifdef __cplusplus extern "C" { @@ -45,7 +42,6 @@ extern "C" { typedef inv_error_t(*state_change_callback_t) (unsigned char newState); - char *inv_state_name(unsigned char x); inv_error_t inv_state_transition(unsigned char newState); unsigned char inv_get_state(void); inv_error_t inv_register_state_callback(state_change_callback_t callback); diff --git a/libsensors/mlsdk/mllite/mlsupervisor.c b/libsensors/mlsdk/mllite/mlsupervisor.c index 2546903..c78ea26 100644 --- a/libsensors/mlsdk/mllite/mlsupervisor.c +++ b/libsensors/mlsdk/mllite/mlsupervisor.c @@ -37,7 +37,6 @@ #include "mltypes.h" #include "mlinclude.h" #include "compass.h" -#include "pressure.h" #include "dmpKey.h" #include "dmpDefault.h" #include "mlstates.h" @@ -368,9 +367,7 @@ inv_error_t inv_accel_compass_supervisor(void) long long tmp[3] = { 0 }; long long tmp64 = 0; unsigned long ctime = inv_get_tick_count(); - if (((inv_get_compass_id() == COMPASS_ID_AK8975) && - ((ctime - polltime) > 20)) || - (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max + if (polltime == 0 || ((ctime - polltime) > 20)) { // 50Hz max if (SUPERVISOR_DEBUG) { MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n"); MPL_LOGV("delta time = %ld\n", ctime - polltime); @@ -461,18 +458,15 @@ inv_error_t inv_accel_compass_supervisor(void) } #ifdef APPLY_COMPASS_FILTER - if (inv_get_compass_id() == COMPASS_ID_YAS530) - { - fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); - fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); - fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); + fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); + fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); + fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); - f.update(&handle, fcin, fcout); + f.update(&handle, fcin, fcout); - inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); - inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); - inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); - } + inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); + inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); + inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); #endif if (SUPERVISOR_DEBUG) { @@ -532,32 +526,6 @@ inv_error_t inv_accel_compass_supervisor(void) } /** - * @brief Entry point for software sensor fusion operations. - * Manages hardware interaction, calls sensor fusion supervisor for - * bias calculation. - * @return INV_SUCCESS or non-zero error code on error. - */ -inv_error_t inv_pressure_supervisor(void) -{ - long pressureSensorData[1]; - static unsigned long pressurePolltime = 0; - if (inv_pressure_present()) { /* check for pressure data */ - unsigned long ctime = inv_get_tick_count(); - if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second - if (SUPERVISOR_DEBUG) { - MPL_LOGV("Fetch pressure data\n"); - MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime); - } - pressurePolltime = ctime; - if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) { - inv_obj.pressure = pressureSensorData[0]; - } - } - } - return INV_SUCCESS; -} - -/** * @brief Resets the magnetometer calibration algorithm. * @return INV_SUCCESS if successful, or non-zero error code otherwise. */ diff --git a/libsensors/mlsdk/mllite/mlsupervisor.h b/libsensors/mlsdk/mllite/mlsupervisor.h index 62b427e..d7c5251 100644 --- a/libsensors/mlsdk/mllite/mlsupervisor.h +++ b/libsensors/mlsdk/mllite/mlsupervisor.h @@ -26,9 +26,6 @@ #define __INV_SUPERVISOR_H__ #include "mltypes.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlsupervisor_legacy.h" -#endif // The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = 2^this_number // this number must be >=0 and even. @@ -65,7 +62,6 @@ struct inv_supervisor_cb_obj { inv_error_t inv_reset_compass_calibration(void); void inv_init_sensor_fusion_supervisor(void); inv_error_t inv_accel_compass_supervisor(void); -inv_error_t inv_pressure_supervisor(void); #endif // __INV_SUPERVISOR_H__ diff --git a/libsensors/mlsdk/mllite/pressure.c b/libsensors/mlsdk/mllite/pressure.c deleted file mode 100644 index f32229f..0000000 --- a/libsensors/mlsdk/mllite/pressure.c +++ /dev/null @@ -1,166 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: pressure.c 4120 2010-11-21 19:56:16Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup PRESSUREDL - * @brief Motion Library - Pressure Driver Layer. - * Provides the interface to setup and handle a pressure sensor - * connected to either the primary or the seconday I2C interface - * of the gyroscope. - * - * @{ - * @file pressure.c - * @brief Pressure setup and handling methods. -**/ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> - -#include "pressure.h" - -#include "ml.h" -#include "mlinclude.h" -#include "dmpKey.h" -#include "mlFIFO.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mlMathFunc.h" -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-pressure" - -#define _pressureDebug(x) //{x} - -/* --------------------- */ -/* - Global Variables. - */ -/* --------------------- */ - -/* --------------------- */ -/* - Static Variables. - */ -/* --------------------- */ - -/* --------------- */ -/* - Prototypes. - */ -/* --------------- */ - -/* -------------- */ -/* - Externs. - */ -/* -------------- */ - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Is a pressure configured and used by MPL? - * @return INV_SUCCESS if the pressure is present. - */ -unsigned char inv_pressure_present(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pressure && - NULL != mldl_cfg->pressure->resume && - mldl_cfg->requested_sensors & INV_THREE_AXIS_PRESSURE) - return TRUE; - else - return FALSE; -} - -/** - * @brief Query the pressure slave address. - * @return The 7-bit pressure slave address. - */ -unsigned char inv_get_pressure_slave_addr(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pdata) - return mldl_cfg->pdata->pressure.address; - else - return 0; -} - -/** - * @brief Get the ID of the pressure in use. - * @return ID of the pressure in use. - */ -unsigned short inv_get_pressure_id(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pressure) { - return mldl_cfg->pressure->id; - } - return ID_INVALID; -} - -/** - * @brief Get a sample of pressure data from the device. - * @param data - * the buffer to store the pressure raw data for - * X, Y, and Z axes. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_pressure_data(long *data) -{ - inv_error_t result = INV_SUCCESS; - unsigned char tmp[3]; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - /*--- read the pressure sensor data. - The pressure read function may return an INV_ERROR_PRESSURE_* errors - when the data is not ready (read/refresh frequency mismatch) or - the internal data sampling timing of the device was not respected. - Returning the error code will make the sensor fusion supervisor - ignore this pressure data sample. ---*/ - result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), tmp); - if (result) { - _pressureDebug(MPL_LOGV - ("inv_mpu_read_pressure returned %d (%s)\n", result, - MLErrorCode(result))); - return result; - } - if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->pressure->endian) - data[0] = - (((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) + - ((long)tmp[2]); - else - data[0] = - (((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) + - ((long)tmp[0]); - - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/libsensors/mlsdk/mllite/pressure.h b/libsensors/mlsdk/mllite/pressure.h deleted file mode 100644 index 77c5d43..0000000 --- a/libsensors/mlsdk/mllite/pressure.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: pressure.h 4092 2010-11-17 23:49:22Z kkeal $ - * - *******************************************************************************/ - -#ifndef PRESSURE_H -#define PRESSURE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "pressure_legacy.h" -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -#define USE_PRESSURE_BMA 0 - -#define PRESSURE_SLAVEADDR_INVALID 0x00 -#define PRESSURE_SLAVEADDR_BMA085 0x77 - -/* - Define default pressure to use if no selection is made -*/ -#if USE_PRESSURE_BMA -#define DEFAULT_PRESSURE_TYPE PRESSURE_ID_BMA -#endif - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - - unsigned char inv_pressure_present(void); - unsigned char inv_get_pressure_slave_addr(void); - inv_error_t inv_suspend_pressure(void); - inv_error_t inv_resume_presure(void); - inv_error_t inv_get_pressure_data(long *data); - unsigned short inv_get_pressure_id(void); - -#ifdef __cplusplus -} -#endif -#endif // PRESSURE_H diff --git a/libsensors/mlsdk/mlutils/checksum.h b/libsensors/mlsdk/mlutils/checksum.h index 4d3f046..db8522e 100644 --- a/libsensors/mlsdk/mlutils/checksum.h +++ b/libsensors/mlsdk/mlutils/checksum.h @@ -6,9 +6,6 @@ extern "C" { #endif #include "mltypes.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "checksum_legacy.h" -#endif uint32_t inv_checksum(unsigned char *str, int len); diff --git a/libsensors/mlsdk/mlutils/mputest.c b/libsensors/mlsdk/mlutils/mputest.c deleted file mode 100644 index 4a598ff..0000000 --- a/libsensors/mlsdk/mlutils/mputest.c +++ /dev/null @@ -1,1101 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $ - * - *****************************************************************************/ - -/* - * MPU Self Test functions - * Version 2.4 - * May 13th, 2011 - */ - -/** - * @defgroup MPU_SELF_TEST - * @brief MPU Self Test functions - * - * These functions provide an in-site test of the MPU 3xxx chips. The main - * entry point is the inv_mpu_test function. - * This runs the tests (as described in the accompanying documentation) and - * writes a configuration file containing initial calibration data. - * inv_mpu_test returns INV_SUCCESS if the chip passes the tests. - * Otherwise, an error code is returned. - * The functions in this file rely on MLSL and MLOS: refer to the MPL - * documentation for more information regarding the system interface - * files. - * - * @{ - * @file mputest.c - * @brief MPU Self Test routines for assessing gyro sensor status - * after surface mount has happened on the target host platform. - */ - -#include <stdio.h> -#include <time.h> -#include <string.h> -#include <math.h> -#include <stdlib.h> -#ifdef LINUX -#include <unistd.h> -#endif - -#include "mpu.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "accel.h" -#include "mlFIFO.h" -#include "slave.h" -#include "ml.h" -#include "ml_stored_data.h" -#include "checksum.h" - -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mpust" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Defines -*/ - -#define VERBOSE_OUT 0 - -/*#define TRACK_IDS*/ - -/*--- Test parameters defaults. See set_test_parameters for more details ---*/ - -#define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */ - -#if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */ -#define DEF_GYRO_FULLSCALE (2000) -#else -#define DEF_GYRO_FULLSCALE (250) -#endif - -#define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE) - /* gyro sensitivity LSB/dps */ -#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ -#define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */ -#define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS) - /* 40 dps in LSBs */ -#define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS) - /* 0.4 dps-rms in LSB-rms */ -#define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */ -#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting - data for each axis, - multiple of 600ms */ -#define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to - average from, if applic. */ - -#define ML_INIT_CAL_LEN (36) /* length in bytes of - calibration data file */ - -/* - Macros -*/ - -#define CHECK_TEST_ERROR(x) \ - if (x) { \ - MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \ - return (-1); \ - } - -#define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f) - -#define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \ - (chr)[1]=(unsigned char)(shrt); - -#define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \ - (chr)[1]=(unsigned char)(ui>>16); \ - (chr)[2]=(unsigned char)(ui>>8); \ - (chr)[3]=(unsigned char)(ui); - -#define FLOAT_TO_SHORT(f) ( \ - (fabs(f-(short)f)>=0.5) ? ( \ - ((short)f)+(f<0?(-1):(+1))) : \ - ((short)f) \ - ) - -#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) -#define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0]) - -#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5] - -#define CHECK_NACKS(d) ( \ - d[0]==0xff && d[1]==0xff && \ - d[2]==0xff && d[3]==0xff && \ - d[4]==0xff && d[5]==0xff \ - ) - -/* - Prototypes -*/ - -static inv_error_t test_get_data( - void *mlsl_handle, - struct mldl_cfg *mputestCfgPtr, - short *vals); - -/* - Types -*/ -typedef struct { - float gyro_sens; - int gyro_fs; - int packet_thresh; - float total_timing_tol; - int bias_thresh; - float rms_threshSq; - float sp_shift_thresh; - unsigned int test_time_per_axis; - unsigned short accel_samples; -} tTestSetup; - -/* - Global variables -*/ -static unsigned char dataout[20]; -static unsigned char dataStore[ML_INIT_CAL_LEN]; - -static tTestSetup test_setup = { - DEF_GYRO_SENS, - DEF_GYRO_FULLSCALE, - DEF_PACKET_THRESH, - DEF_TOTAL_TIMING_TOL, - (int)DEF_BIAS_THRESH, - DEF_RMS_THRESH * DEF_RMS_THRESH, - DEF_SP_SHIFT_THRESH_CUST, - DEF_TEST_TIME_PER_AXIS, - DEF_N_ACCEL_SAMPLES -}; - -static float adjGyroSens; -static char a_name[3][2] = {"X", "Y", "Z"}; - -/* - NOTE : modify get_slave_descr parameter below to reflect - the DEFAULT accelerometer in use. The accelerometer in use - can be modified at run-time using the inv_test_setup_accel API. - NOTE : modify the expected z axis orientation (Z axis pointing - upward or downward) -*/ - -signed char g_z_sign = +1; -struct mldl_cfg *mputestCfgPtr = NULL; - -#ifndef LINUX -/** - * @internal - * @brief usec precision sleep function. - * @param number of micro seconds (us) to sleep for. - */ -static void usleep(unsigned long t) -{ - unsigned long start = inv_get_tick_count(); - while (inv_get_tick_count()-start < t / 1000); -} -#endif - -/** - * @brief Modify the self test limits from their default values. - * - * @param slave_addr - * the slave address the MPU device is setup to respond at. - * The default is DEF_MPU_ADDR = 0x68. - * @param sensitivity - * the read sensitivity of the device in LSB/dps as it is trimmed. - * NOTE : if using the self test as part of the MPL, the - * sensitivity the different sensitivity trims are already - * taken care of. - * @param p_thresh - * number of packets expected to be received in a 600 ms period. - * Depends on the sampling frequency of choice (set by default to - * 125 Hz) and low pass filter cut-off frequency selection (set - * to 42 Hz). - * The default is DEF_PACKET_THRESH = 75 packets. - * @param total_time_tol - * time skew tolerance, taking into account imprecision in turning - * the FIFO on and off and the processor time imprecision (for - * 1 GHz processor). - * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets. - * @param bias_thresh - * bias level threshold, the maximun acceptable no motion bias - * for a production quality part. - * The default is DEF_BIAS_THRESH = 40 dps. - * @param rms_thresh - * the limit standard deviation (=~ RMS) set to assess whether - * the noise level on the part is acceptable. - * The default is DEF_RMS_THRESH = 0.2 dps-rms. - * @param sp_shift_thresh - * the limit shift applicable to the Sense Path self test - * calculation. - */ -void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, - int p_thresh, float total_time_tol, - int bias_thresh, float rms_thresh, - float sp_shift_thresh, - unsigned short accel_samples) -{ - mputestCfgPtr->addr = slave_addr; - test_setup.gyro_sens = sensitivity; - test_setup.gyro_fs = (int)(32768.f / sensitivity); - test_setup.packet_thresh = p_thresh; - test_setup.total_timing_tol = total_time_tol; - test_setup.bias_thresh = bias_thresh; - test_setup.rms_threshSq = rms_thresh * rms_thresh; - test_setup.sp_shift_thresh = sp_shift_thresh; - test_setup.accel_samples = accel_samples; -} - -#define X (0) -#define Y (1) -#define Z (2) - -/** - * @brief Test the gyroscope sensor. - * Implements the core logic of the MPU Self Test. - * Produces the PASS/FAIL result. Loads the calculated gyro biases - * and temperature datum into the corresponding pointers. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param gyro_biases - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements for gyro X, Y, - * and Z. - * @param temp_avg - * output pointer to store the initial average temperature as - * provided by the MPU Self Test. - * @param perform_full_test - * If 1: - * calculates offset, drive frequency, and noise and compare it - * against set thresholds. - * Report also the final result using a bit-mask like error code - * as explained in return value description. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment; simply calculates the gyro biases. - * - * @return 0 on success. - * On error, the return value is a bitmask representing: - * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively - * (decimal values will be 1, 2, 4 respectively). - * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively - * (decimal values will be 8, 16, 32 respectively). - * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively - * (decimal values will be 64, 128, 256 respectively). - * 9 If any of the RMS noise values is zero, it could be - * due to a non-functional gyro or FIFO/register failure. - * (decimal value will be 512). - * (decimal values will be 1024, 2048, 4096 respectively). - */ -int inv_test_gyro_3050(void *mlsl_handle, - short gyro_biases[3], short *temp_avg, - uint_fast8_t perform_full_test) -{ - int retVal = 0; - inv_error_t result; - - int total_count = 0; - int total_count_axis[3] = {0, 0, 0}; - int packet_count; - short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - unsigned char regs[7]; - - int temperature; - float Avg[3]; - float RMS[3]; - int i, j, tmp; - char tmpStr[200]; - - temperature = 0; - - /* sample rate = 8ms */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_SMPLRT_DIV, 0x07); - CHECK_TEST_ERROR(result); - - regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ - switch (DEF_GYRO_FULLSCALE) { - case 2000: - regs[0] |= 0x18; - break; - case 1000: - regs[0] |= 0x10; - break; - case 500: - regs[0] |= 0x08; - break; - case 250: - default: - regs[0] |= 0x00; - break; - } - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_DLPF_FS_SYNC, regs[0]); - CHECK_TEST_ERROR(result); - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_INT_CFG, 0x00); - CHECK_TEST_ERROR(result); - - /* 1st, timing test */ - for (j = 0; j < 3; j++) { - - MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); - - /* turn on all gyros, use gyro X for clocking - Set to Y and Z for 2nd and 3rd iteration */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, j + 1); - CHECK_TEST_ERROR(result); - - /* wait for 2 ms after switching clock source */ - usleep(2000); - - /* we will enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN2, 0x00); - CHECK_TEST_ERROR(result); - /* enable/reset FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); - CHECK_TEST_ERROR(result); - - tmp = (int)(test_setup.test_time_per_axis / 600); - while (tmp-- > 0) { - /* enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write(mlsl_handle, - mputestCfgPtr->addr, MPUREG_FIFO_EN1, - BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); - CHECK_TEST_ERROR(result); - - /* wait for 600 ms for data */ - usleep(600000); - - /* stop storing gyro in the FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN1, 0x00); - CHECK_TEST_ERROR(result); - - /* Getting number of bytes in FIFO */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_COUNTH, 2, dataout); - CHECK_TEST_ERROR(result); - /* number of 6 B packets in the FIFO */ - packet_count = CHARS_TO_SHORT(dataout) / 6; - sprintf(tmpStr, "Packet Count: %d - ", packet_count); - - if ( abs(packet_count - test_setup.packet_thresh) - <= /* Within +/- total_timing_tol % range */ - test_setup.total_timing_tol * test_setup.packet_thresh) { - for (i = 0; i < packet_count; i++) { - /* getting FIFO data */ - result = inv_serial_read_fifo(mlsl_handle, - mputestCfgPtr->addr, 6, dataout); - CHECK_TEST_ERROR(result); - x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); - y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); - z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); - if (VERBOSE_OUT) { - MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", - total_count + i, x[total_count + i], - y[total_count + i], z[total_count + i]); - } - } - total_count += packet_count; - total_count_axis[j] += packet_count; - sprintf(tmpStr, "%sOK", tmpStr); - } else { - if (perform_full_test) - retVal |= 1 << j; - sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); - } - MPL_LOGI("%s\n", tmpStr); - } - - /* remove gyros from FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN1, 0x00); - CHECK_TEST_ERROR(result); - - /* Read Temperature */ - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, - MPUREG_TEMP_OUT_H, 2, dataout); - CHECK_TEST_ERROR(result); - temperature += (short)CHARS_TO_SHORT(dataout); - } - - MPL_LOGI("\n"); - MPL_LOGI("Total %d samples\n", total_count); - MPL_LOGI("\n"); - - /* 2nd, check bias from X and Y PLL clock source */ - tmp = total_count != 0 ? total_count : 1; - for (i = 0, - Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; - i < total_count; i++) { - Avg[X] += 1.f * x[i] / tmp; - Avg[Y] += 1.f * y[i] / tmp; - Avg[Z] += 1.f * z[i] / tmp; - } - MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", - Avg[X], Avg[Y], Avg[Z]); - if (VERBOSE_OUT) { - MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", - Avg[X] / adjGyroSens, - Avg[Y] / adjGyroSens, - Avg[Z] / adjGyroSens); - } - if(perform_full_test) { - for (j = 0; j < 3; j++) { - if (fabs(Avg[j]) > test_setup.bias_thresh) { - MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " - "(threshold = %d)\n", - a_name[j], Avg[j], test_setup.bias_thresh); - retVal |= 1 << (3+j); - } - } - } - - /* 3rd, check RMS */ - if (perform_full_test) { - for (i = 0, - RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; - i < total_count; i++) { - RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); - RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); - RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); - } - for (j = 0; j < 3; j++) { - if (RMS[j] > test_setup.rms_threshSq * total_count) { - MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold " - "(threshold = %.2f)\n", - a_name[j], sqrt(RMS[j] / total_count), - sqrt(test_setup.rms_threshSq)); - retVal |= 1 << (6+j); - } - } - - MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", - sqrt(RMS[X] / total_count), - sqrt(RMS[Y] / total_count), - sqrt(RMS[Z] / total_count)); - if (VERBOSE_OUT) { - MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n", - RMS[X] / total_count, - RMS[Y] / total_count, - RMS[Z] / total_count); - } - - if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { - /* If any of the RMS noise value returns zero, - then we might have dead gyro or FIFO/register failure, - the part is sleeping, or the part is not responsive */ - retVal |= 1 << 9; - } - } - - /* 4th, temperature average */ - temperature /= 3; - if (VERBOSE_OUT) - MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", - SHORT_TO_TEMP_C(temperature), "", ""); - - /* load into final storage */ - *temp_avg = (short)temperature; - gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); - gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); - gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); - - return retVal; -} - -#ifdef TRACK_IDS -/** - * @internal - * @brief Retrieve the unique MPU device identifier from the internal OTP - * bank 0 memory. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @return 0 on success, a non-zero error code from the serial layer on error. - */ -static inv_error_t test_get_mpu_id(void *mlsl_handle) -{ - inv_error_t result; - unsigned char otp0[8]; - - - result = - inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr, - (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 | - 0x00, 6, otp0); - if (result) - goto close; - - MPL_LOGI("\n"); - MPL_LOGI("DIE_ID : %06X\n", - ((int)otp0[1] << 8 | otp0[0]) & 0x1fff); - MPL_LOGI("WAFER_ID : %06X\n", - (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5); - MPL_LOGI("A_LOT_ID : %06X\n", - ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 | - otp0[2]) & 0x3ffff) >> 2); - MPL_LOGI("W_LOT_ID : %06X\n", - ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2); - MPL_LOGI("WP_ID : %06X\n", - ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7); - MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2); - MPL_LOGI("\n"); - -close: - result = - inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00); - return result; -} -#endif /* TRACK_IDS */ - -/** - * @brief If requested via inv_test_setup_accel(), test the accelerometer biases - * and calculate the necessary bias correction. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param bias - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements to store accel X, Y, - * and Z axis bias. - * @param perform_full_test - * If 1: - * calculates offsets and noise and compare it against set - * thresholds. The final exist status will reflect if any of the - * value is outside of the expected range. - * When 0; - * skip the noise calculation and pass/fail assessment; simply - * calculates the accel biases. - * - * @return 0 on success. A non-zero error code on error. - */ -int inv_test_accel(void *mlsl_handle, - short *bias, long gravity, - uint_fast8_t perform_full_test) -{ - int i; - - short *p_vals; - float x = 0.f, y = 0.f, z = 0.f, zg = 0.f; - float RMS[3]; - float accelRmsThresh = 1000000.f; /* enourmous so that the test always - passes - future deployment */ - unsigned short res; - unsigned long orig_requested_sensors; - struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata; - - p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); - - /* load the slave descr from the getter */ - if (mputestPData->accel.get_slave_descr == NULL) { - MPL_LOGI("\n"); - MPL_LOGI("No accelerometer configured\n"); - return 0; - } - if (mputestCfgPtr->accel == NULL) { - MPL_LOGI("\n"); - MPL_LOGI("No accelerometer configured\n"); - return 0; - } - - /* resume the accel */ - orig_requested_sensors = mputestCfgPtr->requested_sensors; - mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO; - res = inv_mpu_resume(mputestCfgPtr, - mlsl_handle, NULL, NULL, NULL, - mputestCfgPtr->requested_sensors); - if(res != INV_SUCCESS) - goto accel_error; - - /* wait at least a sample cycle for the - accel data to be retrieved by MPU */ - inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000); - - /* collect the samples */ - for(i = 0; i < test_setup.accel_samples; i++) { - short *vals = &p_vals[3 * i]; - if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) { - goto accel_error; - } - x += 1.f * vals[X] / test_setup.accel_samples; - y += 1.f * vals[Y] / test_setup.accel_samples; - z += 1.f * vals[Z] / test_setup.accel_samples; - } - - mputestCfgPtr->requested_sensors = orig_requested_sensors; - res = inv_mpu_suspend(mputestCfgPtr, - mlsl_handle, NULL, NULL, NULL, - INV_ALL_SENSORS); - if (res != INV_SUCCESS) - goto accel_error; - - MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z); - if (VERBOSE_OUT) { - MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", - x / gravity, y / gravity, z / gravity); - } - - bias[0] = FLOAT_TO_SHORT(x); - bias[1] = FLOAT_TO_SHORT(y); - zg = z - g_z_sign * gravity; - bias[2] = FLOAT_TO_SHORT(zg); - - MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", - bias[0], bias[1], bias[2]); - if (VERBOSE_OUT) { - MPL_LOGI("Accel correct.: " - "%+13.3f %+13.3f %+13.3f (gee)\n", - 1.f * bias[0] / gravity, - 1.f * bias[1] / gravity, - 1.f * bias[2] / gravity); - } - - if (perform_full_test) { - /* accel RMS - for now the threshold is only indicative */ - for (i = 0, - RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; - i < test_setup.accel_samples; i++) { - short *vals = &p_vals[3 * i]; - RMS[X] += (vals[X] - x) * (vals[X] - x); - RMS[Y] += (vals[Y] - y) * (vals[Y] - y); - RMS[Z] += (vals[Z] - z) * (vals[Z] - z); - } - for (i = 0; i < 3; i++) { - if (RMS[i] > accelRmsThresh * accelRmsThresh - * test_setup.accel_samples) { - MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " - "(threshold = %.2f)\n", - a_name[i], sqrt(RMS[i] / test_setup.accel_samples), - accelRmsThresh); - goto accel_error; - } - } - MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", - sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES), - sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES), - sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES)); - } - - return 0; /* success */ - -accel_error: /* error */ - bias[0] = bias[1] = bias[2] = 0; - return 1; -} - -/** - * @brief an user-space substitute of the power management function(s) - * in mldl_cfg.c for self test usage. - * Wake up and sleep the device, whether that is MPU3050, MPU6050A2, - * or MPU6050B1. - * @param mlsl_handle - * a file handle for the serial communication port used to - * communicate with the MPU device. - * @param power_level - * the power state to change the device into. Currently only 0 or - * 1 are supported, for sleep and full-power respectively. - * @return 0 on success; a non-zero error code on error. - */ -static inv_error_t inv_device_power_mgmt(void *mlsl_handle, - uint_fast8_t power_level) -{ - inv_error_t result; - static unsigned char pwr_mgm; - unsigned char b; - - if (power_level != 0 && power_level != 1) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (power_level) { - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 1, &pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* reset */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET); - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - inv_sleep(5); - - /* re-read power mgmt reg - - it may have reset after H_RESET is applied */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 1, &b); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* wake up */ - if (pwr_mgm & BIT_SLEEP) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 0x00); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - inv_sleep(60); - } else { - /* restore the power state the part was found in */ - if (pwr_mgm & BIT_SLEEP) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - - return INV_SUCCESS; -} - -/** - * @brief The main entry point of the MPU Self Test, triggering the run of - * the single tests, for gyros and accelerometers. - * Prepares the MPU for the test, taking the device out of low power - * state if necessary, switching the MPU secondary I2C interface into - * bypass mode and restoring the original power state at the end of - * the test. - * This function is also responsible for encoding the output of each - * test in the correct format as it is stored on the file/medium of - * choice (according to inv_serial_write_cal() function). - * The format needs to stay perfectly consistent with the one expected - * by the corresponding loader in ml_stored_data.c; currectly the - * loaded in use is inv_load_cal_V1 (record type 1 - initial - * calibration). - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set threshouds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro() function. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * NOTE: for MPU6050 devices, this parameter is currently - * ignored. - * - * @return 0 on success. A non-zero error code on error. - * Propagates the errors from the tests up to the caller. - */ -int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result) -{ - int result = 0; - - short temp_avg; - short gyro_biases[3] = {0, 0, 0}; - short accel_biases[3] = {0, 0, 0}; - - unsigned long testStart = inv_get_tick_count(); - long accelSens[3] = {0}; - int ptr; - int tmp; - long long lltmp; - uint32_t chk; - - MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n", - DEF_TEST_TIME_PER_AXIS / 600); - MPL_LOGI("\n"); - - result = inv_device_power_mgmt(mlsl_handle, TRUE); - -#ifdef TRACK_IDS - result = test_get_mpu_id(mlsl_handle); - if (result != INV_SUCCESS) { - MPL_LOGI("Could not read the device's unique ID\n"); - MPL_LOGI("\n"); - return result; - } -#endif - - /* adjust the gyro sensitivity according to the gyro_sens_trim value */ - adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f; - test_setup.gyro_fs = (int)(32768.f / adjGyroSens); - - /* collect gyro and temperature data */ - result = inv_test_gyro_3050(mlsl_handle, - gyro_biases, &temp_avg, provide_result); - - MPL_LOGI("\n"); - MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart); - if (result) - return result; - - /* collect accel data. if this step is skipped, - ensure the array still contains zeros. */ - if (mputestCfgPtr->accel != NULL) { - float fs; - RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs); - accelSens[0] = (long)(32768L / fs); - accelSens[1] = (long)(32768L / fs); - accelSens[2] = (long)(32768L / fs); - } else { - /* would be 0, but 1 to avoid divide-by-0 below */ - accelSens[0] = accelSens[1] = accelSens[2] = 1; - } - result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], - provide_result); - if (result) - return result; - - result = inv_device_power_mgmt(mlsl_handle, FALSE); - if (result) - return result; - - ptr = 0; - dataStore[ptr++] = 0; /* total len of factory cal */ - dataStore[ptr++] = 0; - dataStore[ptr++] = 0; - dataStore[ptr++] = ML_INIT_CAL_LEN; - dataStore[ptr++] = 0; - dataStore[ptr++] = 5; /* record type 5 - initial calibration */ - - tmp = temp_avg; /* temperature */ - if (tmp < 0) tmp += 2 << 16; - USHORT_TO_CHARS(&dataStore[ptr], tmp); - ptr += 2; - - /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */ - lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - - lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - - /* add a checksum for data */ - chk = inv_checksum( - dataStore + INV_CAL_HDR_LEN, - ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN); - UINT_TO_CHARS(&dataStore[ptr], chk); - ptr += 4; - - if (ptr != ML_INIT_CAL_LEN) { - MPL_LOGI("Invalid calibration data length: exp %d, got %d\n", - ML_INIT_CAL_LEN, ptr); - return -1; - } - - return result; -} - -/** - * @brief The main test API. Runs the MPU Self Test and, if successful, - * stores the encoded initial calibration data on the final storage - * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE - * define in your mlsl implementation). - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set threshouds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro() function. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * - * @return 0 on success or a non-zero error code from the callees on error. - */ -inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result) -{ - int result; - - result = inv_mpu_test(mlsl_handle, provide_result); - if (provide_result) { - MPL_LOGI("\n"); - if (result == 0) { - MPL_LOGI("Test : PASSED\n"); - } else { - MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result); - return result; /* abort writing the calibration if the - test is not successful */ - } - MPL_LOGI("\n"); - } else { - MPL_LOGI("\n"); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN); - if (result) { - MPL_LOGI("Error : cannot write calibration on file - error %d\n", - result); - return result; - } - - return INV_SUCCESS; -} - - - -/* ----------------------------------------------------------------------- - accel interface functions - -----------------------------------------------------------------------*/ - -/** - * @internal - * @brief Reads data for X, Y, and Z axis from the accelerometer device. - * Used only if an accelerometer has been setup using the - * inv_test_setup_accel() API. - * Takes care of the accelerometer endianess according to how the - * device has been described in the corresponding accelerometer driver - * file. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param slave - * a pointer to the descriptor of the slave accelerometer device - * in use. Contains the necessary information to operate, read, - * and communicate with the accelerometer device of choice. - * See the declaration of struct ext_slave_descr in mpu.h. - * @param pdata - * a pointer to the platform info of the slave accelerometer - * device in use. Describes how the device is oriented and - * mounted on host platform's PCB. - * @param vals - * output pointer to return the accelerometer's X, Y, and Z axis - * sensor data collected. - * @return 0 on success or a non-zero error code on error. - */ -static inv_error_t test_get_data( - void *mlsl_handle, - struct mldl_cfg *mputestCfgPtr, - short *vals) -{ - inv_error_t result; - unsigned char data[20]; - struct ext_slave_descr *slave = mputestCfgPtr->accel; - - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, - 6, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (VERBOSE_OUT) { - MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n", - ACCEL_UNPACK(data)); - } - - if (CHECK_NACKS(data)) { - MPL_LOGI("Error fetching data from the accelerometer : " - "all bytes read 0xff\n"); - return INV_ERROR_SERIAL_READ; - } - - if (slave->endian == EXT_SLAVE_BIG_ENDIAN) { - vals[0] = CHARS_TO_SHORT(&data[0]); - vals[1] = CHARS_TO_SHORT(&data[2]); - vals[2] = CHARS_TO_SHORT(&data[4]); - } else { - vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]); - vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]); - vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]); - } - - if (VERBOSE_OUT) { - MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", - vals[0], vals[1], vals[2]); - } - return INV_SUCCESS; -} - -#ifdef __cplusplus -} -#endif - -/** - * @} - */ - diff --git a/libsensors/mlsdk/mlutils/mputest.h b/libsensors/mlsdk/mlutils/mputest.h deleted file mode 100644 index d3347c5..0000000 --- a/libsensors/mlsdk/mlutils/mputest.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mputest.h 4051 2010-11-19 04:51:58Z mcaramello $ - * - *****************************************************************************/ - -#ifndef MPUTEST_H -#define MPUTEST_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mlsl.h" -#include "mldl_cfg.h" -#include "mputest_legacy.h" - -/* user facing APIs */ -inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result); -void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, - int p_thresh, float total_time_tol, - int bias_thresh, float rms_thresh, - float sp_shift_thresh, - unsigned short accel_samples); - -/* additional functions */ -int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result); - - -#ifdef __cplusplus -} -#endif - -#endif /* MPUTEST_H */ - |