summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--libsensors/MPLSensor.cpp1
-rw-r--r--libsensors/mlsdk/Android.mk12
-rw-r--r--libsensors/mlsdk/mllite/accel.c28
-rw-r--r--libsensors/mlsdk/mllite/accel.h5
-rw-r--r--libsensors/mlsdk/mllite/compass.c95
-rw-r--r--libsensors/mlsdk/mllite/compass.h7
-rw-r--r--libsensors/mlsdk/mllite/dmpDefaultMantis.c467
-rw-r--r--libsensors/mlsdk/mllite/invensense.h2
-rw-r--r--libsensors/mlsdk/mllite/ml.c77
-rw-r--r--libsensors/mlsdk/mllite/ml.h75
-rw-r--r--libsensors/mlsdk/mllite/mlBiasNoMotion.c21
-rw-r--r--libsensors/mlsdk/mllite/mlFIFO.c6
-rw-r--r--libsensors/mlsdk/mllite/mlFIFO.h3
-rw-r--r--libsensors/mlsdk/mllite/mlFIFOHW.h3
-rw-r--r--libsensors/mlsdk/mllite/mlMathFunc.c177
-rw-r--r--libsensors/mlsdk/mllite/mlMathFunc.h25
-rw-r--r--libsensors/mlsdk/mllite/ml_mputest.c180
-rw-r--r--libsensors/mlsdk/mllite/ml_mputest.h49
-rw-r--r--libsensors/mlsdk/mllite/ml_stored_data.c32
-rw-r--r--libsensors/mlsdk/mllite/mlarray_legacy.c587
-rw-r--r--libsensors/mlsdk/mllite/mlcompat.c315
-rw-r--r--libsensors/mlsdk/mllite/mlcompat.h69
-rw-r--r--libsensors/mlsdk/mllite/mlcontrol.c670
-rw-r--r--libsensors/mlsdk/mllite/mlcontrol.h26
-rw-r--r--libsensors/mlsdk/mllite/mldl.c71
-rw-r--r--libsensors/mlsdk/mllite/mldl.h15
-rw-r--r--libsensors/mlsdk/mllite/mldl_cfg.h1
-rw-r--r--libsensors/mlsdk/mllite/mldmp.h3
-rw-r--r--libsensors/mlsdk/mllite/mlstates.c6
-rw-r--r--libsensors/mlsdk/mllite/mlstates.h4
-rw-r--r--libsensors/mlsdk/mllite/mlsupervisor.c48
-rw-r--r--libsensors/mlsdk/mllite/mlsupervisor.h4
-rw-r--r--libsensors/mlsdk/mllite/pressure.c166
-rw-r--r--libsensors/mlsdk/mllite/pressure.h71
-rw-r--r--libsensors/mlsdk/mlutils/checksum.h3
-rw-r--r--libsensors/mlsdk/mlutils/mputest.c1101
-rw-r--r--libsensors/mlsdk/mlutils/mputest.h54
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 */
-