am 086bbb4e: am a38914db: Invensense: 6515: adjust minDelay and maxDelay values
* commit '086bbb4e8cf132cf2398d25c34c0e1533edf5ec1':
Invensense: 6515: adjust minDelay and maxDelay values
diff --git a/60xx/Android.mk b/60xx/Android.mk
deleted file mode 100644
index 427bbb4..0000000
--- a/60xx/Android.mk
+++ /dev/null
@@ -1,8 +0,0 @@
-# Can't have both manta and non-manta libsensors.
-ifneq ($(filter manta, $(TARGET_DEVICE)),)
-# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta.
-include $(call all-named-subdir-makefiles,libsensors_iio)
-else
-# libsensors expects an non-IIO MPU3050.
-include $(call all-named-subdir-makefiles,mlsdk libsensors)
-endif
diff --git a/60xx/libsensors/Android.mk b/60xx/libsensors/Android.mk
deleted file mode 100644
index 326e096..0000000
--- a/60xx/libsensors/Android.mk
+++ /dev/null
@@ -1,47 +0,0 @@
-# Copyright (C) 2008 The Android Open Source Project
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-# Modified 2011 by InvenSense, Inc
-
-
-LOCAL_PATH := $(call my-dir)
-
-ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false)
-
-# InvenSense fragment of the HAL
-include $(CLEAR_VARS)
-
-LOCAL_MODULE := libinvensense_hal
-
-LOCAL_MODULE_TAGS := optional
-
-LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall
-LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1
-
-LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp
-
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include/linux
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/linux
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mllite
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mldmp
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/aichi
-LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/akmd
-
-LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform
-LOCAL_CPPFLAGS+=-DLINUX=1
-LOCAL_LDFLAGS:=-rdynamic
-
-include $(BUILD_SHARED_LIBRARY)
-
-endif
diff --git a/60xx/libsensors/MPLSensor.cpp b/60xx/libsensors/MPLSensor.cpp
deleted file mode 100644
index 4676d0e..0000000
--- a/60xx/libsensors/MPLSensor.cpp
+++ /dev/null
@@ -1,1304 +0,0 @@
-/*
- * Copyright (C) 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.
- */
-
-//#define LOG_NDEBUG 0
-//see also the EXTRA_VERBOSE define, below
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <float.h>
-#include <poll.h>
-#include <unistd.h>
-#include <dirent.h>
-#include <stdlib.h>
-#include <sys/select.h>
-#include <dlfcn.h>
-#include <pthread.h>
-
-#include <cutils/log.h>
-#include <utils/KeyedVector.h>
-
-#include "MPLSensor.h"
-
-#include "math.h"
-#include "ml.h"
-#include "mlFIFO.h"
-#include "mlsl.h"
-#include "mlos.h"
-#include "ml_mputest.h"
-#include "ml_stored_data.h"
-#include "mldl_cfg.h"
-#include "mldl.h"
-
-#include "mpu.h"
-#include "accel.h"
-#include "compass.h"
-#include "kernel/timerirq.h"
-#include "kernel/mpuirq.h"
-#include "kernel/slaveirq.h"
-
-extern "C" {
-#include "mlsupervisor.h"
-}
-
-#include "mlcontrol.h"
-#include "sensor_params.h"
-
-#define EXTRA_VERBOSE (0)
-//#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__)
-#define FUNC_LOG
-#define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__)
-/* this mask must turn on only the sensors that are present and managed by the MPL */
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO)
-
-#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember))
-
-/******************************************/
-
-/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */
-static struct sensor_t sSensorList[] =
- { { "MPL Gyroscope", "Invensense", 1,
- SENSORS_GYROSCOPE_HANDLE,
- SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
- { "MPL Accelerometer", "Invensense", 1,
- SENSORS_ACCELERATION_HANDLE,
- SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
- { "MPL Magnetic Field", "Invensense", 1,
- SENSORS_MAGNETIC_FIELD_HANDLE,
- SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
- { "MPL Orientation", "Invensense", 1,
- SENSORS_ORIENTATION_HANDLE,
- SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, { } },
- { "MPL Rotation Vector", "Invensense", 1,
- SENSORS_ROTATION_VECTOR_HANDLE,
- SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
- { "MPL Linear Acceleration", "Invensense", 1,
- SENSORS_LINEAR_ACCEL_HANDLE,
- SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
- { "MPL Gravity", "Invensense", 1,
- SENSORS_GRAVITY_HANDLE,
- SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
-};
-
-static unsigned long long irq_timestamp = 0;
-/* ***************************************************************************
- * MPL interface misc.
- */
-//static pointer to the object that will handle callbacks
-static MPLSensor* gMPLSensor = NULL;
-
-/* we need to pass some callbacks to the MPL. The mpl is a C library, so
- * wrappers for the C++ callback implementations are required.
- */
-extern "C" {
-//callback wrappers go here
-void mot_cb_wrapper(uint16_t val)
-{
- if (gMPLSensor) {
- gMPLSensor->cbOnMotion(val);
- }
-}
-
-void procData_cb_wrapper()
-{
- if(gMPLSensor) {
- gMPLSensor->cbProcData();
- }
-}
-
-} //end of extern C
-
-void setCallbackObject(MPLSensor* gbpt)
-{
- gMPLSensor = gbpt;
-}
-
-
-/*****************************************************************************
- * sensor class implementation
- */
-
-#define GY_ENABLED ((1<<ID_GY) & enabled_sensors)
-#define A_ENABLED ((1<<ID_A) & enabled_sensors)
-#define O_ENABLED ((1<<ID_O) & enabled_sensors)
-#define M_ENABLED ((1<<ID_M) & enabled_sensors)
-#define LA_ENABLED ((1<<ID_LA) & enabled_sensors)
-#define GR_ENABLED ((1<<ID_GR) & enabled_sensors)
-#define RV_ENABLED ((1<<ID_RV) & enabled_sensors)
-
-MPLSensor::MPLSensor() :
- SensorBase(NULL, NULL),
- mNewData(0),
- mDmpStarted(false),
- mMasterSensorMask(INV_ALL_SENSORS),
- mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
- mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
- mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
- mUseTimerirq(false), mSampleCount(0),
- mEnabled(0), mPendingMask(0)
-{
- FUNC_LOG;
- int mpu_int_fd;
- char *port = NULL;
-
- ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors);
-
- pthread_mutex_init(&mMplMutex, NULL);
-
- mForceSleep = false;
-
- /* used for identifying whether 9axis is enabled or not */
- /* this variable will be changed in initMPL() when libmpl is loaded */
- /* sensor list will be changed based on this variable */
- mNineAxisEnabled = false;
-
- for (size_t i = 0; i < ARRAY_SIZE(mPollFds); i++) {
- mPollFds[i].fd = -1;
- mPollFds[i].events = 0;
- }
-
- pthread_mutex_lock(&mMplMutex);
-
- mpu_int_fd = open("/dev/mpuirq", O_RDWR);
- if (mpu_int_fd == -1) {
- ALOGE("could not open the mpu irq device node");
- } else {
- fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK);
- mIrqFds.add(MPUIRQ_FD, mpu_int_fd);
- mPollFds[MPUIRQ_FD].fd = mpu_int_fd;
- mPollFds[MPUIRQ_FD].events = POLLIN;
- }
-
- accel_fd = open("/dev/accelirq", O_RDWR);
- if (accel_fd == -1) {
- ALOGE("could not open the accel irq device node");
- } else {
- fcntl(accel_fd, F_SETFL, O_NONBLOCK);
- mIrqFds.add(ACCELIRQ_FD, accel_fd);
- mPollFds[ACCELIRQ_FD].fd = accel_fd;
- mPollFds[ACCELIRQ_FD].events = POLLIN;
- }
-
- timer_fd = open("/dev/timerirq", O_RDWR);
- if (timer_fd == -1) {
- ALOGE("could not open the timer irq device node");
- } else {
- fcntl(timer_fd, F_SETFL, O_NONBLOCK);
- mIrqFds.add(TIMERIRQ_FD, timer_fd);
- mPollFds[TIMERIRQ_FD].fd = timer_fd;
- mPollFds[TIMERIRQ_FD].events = POLLIN;
- }
-
- data_fd = mpu_int_fd;
-
- if ((accel_fd == -1) && (timer_fd != -1)) {
- //no accel irq and timer available
- mUseTimerIrqAccel = true;
- }
-
- memset(mPendingEvents, 0, sizeof(mPendingEvents));
-
- mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
- mPendingEvents[RotationVector].sensor = ID_RV;
- mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
-
- mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
- mPendingEvents[LinearAccel].sensor = ID_LA;
- mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
-
- mPendingEvents[Gravity].version = sizeof(sensors_event_t);
- mPendingEvents[Gravity].sensor = ID_GR;
- mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
-
- mPendingEvents[Gyro].version = sizeof(sensors_event_t);
- mPendingEvents[Gyro].sensor = ID_GY;
- mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
-
- mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
- mPendingEvents[Accelerometer].sensor = ID_A;
- mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
-
- mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
- mPendingEvents[MagneticField].sensor = ID_M;
- mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
- mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
-
- mPendingEvents[Orientation].version = sizeof(sensors_event_t);
- mPendingEvents[Orientation].sensor = ID_O;
- mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
- mPendingEvents[Orientation].orientation.status
- = SENSOR_STATUS_ACCURACY_HIGH;
-
- mHandlers[RotationVector] = &MPLSensor::rvHandler;
- mHandlers[LinearAccel] = &MPLSensor::laHandler;
- mHandlers[Gravity] = &MPLSensor::gravHandler;
- mHandlers[Gyro] = &MPLSensor::gyroHandler;
- mHandlers[Accelerometer] = &MPLSensor::accelHandler;
- mHandlers[MagneticField] = &MPLSensor::compassHandler;
- mHandlers[Orientation] = &MPLSensor::orienHandler;
-
- for (int i = 0; i < numSensors; i++)
- mDelays[i] = 30000000LLU; // 30 ms by default
-
- if (inv_serial_start(port) != INV_SUCCESS) {
- ALOGE("Fatal Error : could not open MPL serial interface");
- }
-
- //initialize library parameters
- initMPL();
-
- //setup the FIFO contents
- setupFIFO();
-
-
- pthread_mutex_unlock(&mMplMutex);
-
-}
-
-MPLSensor::~MPLSensor()
-{
- FUNC_LOG;
- pthread_mutex_lock(&mMplMutex);
- if (inv_dmp_stop() != INV_SUCCESS) {
- ALOGW("Error: could not stop the DMP correctly.\n");
- }
-
- if (inv_dmp_close() != INV_SUCCESS) {
- ALOGW("Error: could not close the DMP");
- }
-
- if (inv_serial_stop() != INV_SUCCESS) {
- ALOGW("Error : could not close the serial port");
- }
- pthread_mutex_unlock(&mMplMutex);
- pthread_mutex_destroy(&mMplMutex);
-}
-
-/* clear any data from our various filehandles */
-void MPLSensor::clearIrqData(bool* irq_set)
-{
- unsigned int i;
- int nread;
- struct mpuirq_data irqdata;
-
- poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared
-
- for (i = 0; i < ARRAY_SIZE(mPollFds); i++) {
- int cur_fd = mPollFds[i].fd;
- if (mPollFds[i].revents & POLLIN) {
- nread = read(cur_fd, &irqdata, sizeof(irqdata));
- if (nread > 0) {
- irq_set[i] = true;
- irq_timestamp = irqdata.irqtime;
- }
- }
- mPollFds[i].revents = 0;
- }
-}
-
-/* set the power states of the various sensors based on the bits set in the
- * enabled_sensors parameter.
- * this function modifies globalish state variables. It must be called with the mMplMutex held. */
-void MPLSensor::setPowerStates(int enabled_sensors)
-{
- FUNC_LOG;
- bool irq_set[5] = { false, false, false, false, false };
-
- do {
-
- if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
- mLocalSensorMask = ALL_MPL_SENSORS_NP;
- break;
- }
-
- if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
- mLocalSensorMask = 0;
- break;
- }
-
- if (GY_ENABLED) {
- mLocalSensorMask |= INV_THREE_AXIS_GYRO;
- } else {
- mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
- }
-
- if (A_ENABLED) {
- mLocalSensorMask |= (INV_THREE_AXIS_ACCEL);
- } else {
- mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL);
- }
-
- if (M_ENABLED) {
- mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
- } else {
- mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS);
- }
-
- } while (0);
-
- //record the new sensor state
- inv_error_t rv;
-
- unsigned long sen_mask = mLocalSensorMask & mMasterSensorMask;
-
- bool changing_sensors = ((inv_get_dl_config()->requested_sensors
- != sen_mask) && (sen_mask != 0));
- bool restart = (!mDmpStarted) && (sen_mask != 0);
-
- if (changing_sensors || restart) {
-
- ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart);
-
- if (mDmpStarted) {
- inv_dmp_stop();
- clearIrqData(irq_set);
- mDmpStarted = false;
- }
-
- if (sen_mask != inv_get_dl_config()->requested_sensors) {
- rv = inv_set_mpu_sensors(sen_mask);
- ALOGE_IF(rv != INV_SUCCESS,
- "error: unable to set MPL sensor power states (sens=%ld retcode = %d)",
- sen_mask, rv);
- }
-
- if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS))
- || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL)))
- && ((sen_mask & INV_DMP_PROCESSOR) == 0)) {
- ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
- mUseTimerirq = true;
- } else {
- if (mUseTimerirq) {
- ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
- clearIrqData(irq_set);
- }
- ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
- mUseTimerirq = false;
- }
-
- if (!mDmpStarted) {
- if (mHaveGoodMpuCal || mHaveGoodCompassCal) {
- rv = inv_store_calibration();
- ALOGE_IF(rv != INV_SUCCESS,
- "error: unable to store MPL calibration file");
- mHaveGoodMpuCal = false;
- mHaveGoodCompassCal = false;
- }
- rv = inv_dmp_start();
- ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
- mDmpStarted = true;
- }
- }
-
- //check if we should stop the DMP
- if (mDmpStarted && (sen_mask == 0)) {
- rv = inv_dmp_stop();
- ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)",
- rv);
- if (mUseTimerirq) {
- ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
- }
- clearIrqData(irq_set);
-
- mDmpStarted = false;
- mPollTime = -1;
- mCurFifoRate = -1;
- }
-
-}
-
-/**
- * container function for all the calls we make once to set up the MPL.
- */
-void MPLSensor::initMPL()
-{
- FUNC_LOG;
- inv_error_t result;
- unsigned short bias_update_mask = 0xFFFF;
-
- if (inv_dmp_open() != INV_SUCCESS) {
- ALOGE("Fatal Error : could not open DMP correctly.\n");
- }
-
- result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work
- ALOGE_IF(result != INV_SUCCESS,
- "Fatal Error : could not set enabled sensors.");
-
- if (inv_load_calibration() != INV_SUCCESS) {
- ALOGE("could not open MPL calibration file");
- }
-
- //check for the 9axis fusion library: if available load it and start 9x
- void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW);
- if(h_dmp_lib) {
- const char* error;
- error = dlerror();
- inv_error_t (*fp_inv_enable_9x_fusion)() =
- (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion");
- if((error = dlerror()) != NULL) {
- ALOGE("%s %s", error, "inv_enable_9x_fusion");
- } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) {
- ALOGE( "Warning : 9 axis sensor fusion not available "
- "- No compass detected.\n");
- } else {
- /* 9axis is loaded and enabled */
- /* this variable is used for coming up with sensor list */
- mNineAxisEnabled = true;
- }
- } else {
- const char* error = dlerror();
- ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error);
- }
-
- if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
- ALOGE("Error : Bias update function could not be set.\n");
- }
-
- if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
- ALOGE("Error : could not set motion interrupt");
- }
-
- if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
- ALOGE("Error : could not set fifo interrupt");
- }
-
- result = inv_set_fifo_rate(6);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
- }
-
- setupCallbacks();
-
-}
-
-/** setup the fifo contents.
- */
-void MPLSensor::setupFIFO()
-{
- FUNC_LOG;
- inv_error_t result;
-
- result = inv_send_accel(INV_ALL, INV_32_BIT);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_send_accel returned %d\n", result);
- }
-
- result = inv_send_quaternion(INV_32_BIT);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_send_quaternion returned %d\n", result);
- }
-
- result = inv_send_linear_accel(INV_ALL, INV_32_BIT);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result);
- }
-
- result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n",
- result);
- }
-
- result = inv_send_gravity(INV_ALL, INV_32_BIT);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_send_gravity returned %d\n", result);
- }
-
- result = inv_send_gyro(INV_ALL, INV_32_BIT);
- if (result != INV_SUCCESS) {
- ALOGE("Fatal error: inv_send_gyro returned %d\n", result);
- }
-
-}
-
-/**
- * set up the callbacks that we use in all cases (outside of gestures, etc)
- */
-void MPLSensor::setupCallbacks()
-{
- FUNC_LOG;
- if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) {
- ALOGE("Error : Motion callback could not be set.\n");
-
- }
-
- if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) {
- ALOGE("Error : Processed data callback could not be set.");
-
- }
-}
-
-/**
- * handle the motion/no motion output from the MPL.
- */
-void MPLSensor::cbOnMotion(uint16_t val)
-{
- FUNC_LOG;
- //after the first no motion, the gyro should be calibrated well
- if (val == 2) {
- if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) {
- //if gyros are on and we got a no motion, set a flag
- // indicating that the cal file can be written.
- mHaveGoodMpuCal = true;
- }
- }
-
- return;
-}
-
-
-void MPLSensor::cbProcData()
-{
- mNewData = 1;
- mSampleCount++;
-}
-
-//these handlers transform mpl data into one of the Android sensor types
-// scaling and coordinate transforms should be done in the handlers
-
-void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index)
-{
- VFUNC_LOG;
- inv_error_t res;
- res = inv_get_float_array(INV_GYROS, s->gyro.v);
- s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0;
- s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0;
- s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0;
- if (res == INV_SUCCESS)
- *pending_mask |= (1 << index);
-}
-
-void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index)
-{
- //VFUNC_LOG;
- inv_error_t res;
- res = inv_get_float_array(INV_ACCELS, s->acceleration.v);
- //res = inv_get_accel_float(s->acceleration.v);
- s->acceleration.v[0] = s->acceleration.v[0] * 9.81;
- s->acceleration.v[1] = s->acceleration.v[1] * 9.81;
- s->acceleration.v[2] = s->acceleration.v[2] * 9.81;
- if (res == INV_SUCCESS)
- *pending_mask |= (1 << index);
-}
-
-int MPLSensor::estimateCompassAccuracy()
-{
- inv_error_t res;
- int rv;
-
- res = inv_get_compass_accuracy(&rv);
- if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) {
- mHaveGoodCompassCal = true;
- }
- ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy");
-
- return rv;
-}
-
-void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index)
-{
- VFUNC_LOG;
- inv_error_t res;
-
- res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v);
-
- if (res != INV_SUCCESS) {
- ALOGW(
- "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d",
- res);
- }
-
- s->magnetic.status = estimateCompassAccuracy();
-
- if (res == INV_SUCCESS)
- *pending_mask |= (1 << index);
-}
-
-void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index)
-{
- VFUNC_LOG;
- float quat[4];
- float norm = 0;
- inv_error_t r;
-
- r = inv_get_float_array(INV_QUATERNION, quat);
-
- if (r != INV_SUCCESS) {
- *pending_mask &= ~(1 << index);
- return;
- } else {
- *pending_mask |= (1 << index);
- }
-
- norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]
- + FLT_EPSILON;
-
- if (norm > 1.0f) {
- //renormalize
- norm = sqrtf(norm);
- float inv_norm = 1.0f / norm;
- quat[1] = quat[1] * inv_norm;
- quat[2] = quat[2] * inv_norm;
- quat[3] = quat[3] * inv_norm;
- }
-
- if (quat[0] < 0.0) {
- quat[1] = -quat[1];
- quat[2] = -quat[2];
- quat[3] = -quat[3];
- }
-
- s->gyro.v[0] = quat[1];
- s->gyro.v[1] = quat[2];
- s->gyro.v[2] = quat[3];
-
-}
-
-void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index)
-{
- VFUNC_LOG;
- inv_error_t res;
- res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v);
- s->gyro.v[0] *= 9.81;
- s->gyro.v[1] *= 9.81;
- s->gyro.v[2] *= 9.81;
- if (res == INV_SUCCESS)
- *pending_mask |= (1 << index);
-}
-
-void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index)
-{
- VFUNC_LOG;
- inv_error_t res;
- res = inv_get_float_array(INV_GRAVITY, s->gyro.v);
- s->gyro.v[0] *= 9.81;
- s->gyro.v[1] *= 9.81;
- s->gyro.v[2] *= 9.81;
- if (res == INV_SUCCESS)
- *pending_mask |= (1 << index);
-}
-
-void MPLSensor::calcOrientationSensor(float *R, float *values)
-{
- float tmp;
-
- //Azimuth
- if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) {
- values[0] = (float) atan2f(-R[3], R[0]);
- } else {
- values[0] = (float) atan2f(R[1], R[4]);
- }
- values[0] *= 57.295779513082320876798154814105f;
- if (values[0] < 0) {
- values[0] += 360.0f;
- }
- //Pitch
- tmp = R[7];
- if (tmp > 1.0f)
- tmp = 1.0f;
- if (tmp < -1.0f)
- tmp = -1.0f;
- values[1] = -asinf(tmp) * 57.295779513082320876798154814105f;
- if (R[8] < 0) {
- values[1] = 180.0f - values[1];
- }
- if (values[1] > 180.0f) {
- values[1] -= 360.0f;
- }
- //Roll
- if ((R[7] > 0.7071067f)) {
- values[2] = (float) atan2f(R[6], R[7]);
- } else {
- values[2] = (float) atan2f(R[6], R[8]);
- }
-
- values[2] *= 57.295779513082320876798154814105f;
- if (values[2] > 90.0f) {
- values[2] = 180.0f - values[2];
- }
- if (values[2] < -90.0f) {
- values[2] = -180.0f - values[2];
- }
-}
-
-void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask,
- int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output
-{
- VFUNC_LOG;
- inv_error_t res;
- float rot_mat[9];
-
- res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat);
-
- calcOrientationSensor(rot_mat, s->orientation.v);
-
- s->orientation.status = estimateCompassAccuracy();
-
- if (res == INV_SUCCESS)
- *pending_mask |= (1 << index);
- else
- ALOGW("orienHandler: data not valid (%d)", (int) res);
-
-}
-
-int MPLSensor::enable(int32_t handle, int en)
-{
- FUNC_LOG;
-
- int what = -1;
-
- switch (handle) {
- case ID_A:
- what = Accelerometer;
- break;
- case ID_M:
- what = MagneticField;
- break;
- case ID_O:
- what = Orientation;
- break;
- case ID_GY:
- what = Gyro;
- break;
- case ID_GR:
- what = Gravity;
- break;
- case ID_RV:
- what = RotationVector;
- break;
- case ID_LA:
- what = LinearAccel;
- break;
- default: //this takes care of all the gestures
- what = handle;
- break;
- }
-
- if (uint32_t(what) >= numSensors)
- return -EINVAL;
-
- int newState = en ? 1 : 0;
- int err = 0;
-
- pthread_mutex_lock(&mMplMutex);
- if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
- short flags = newState;
- mEnabled &= ~(1 << what);
- mEnabled |= (uint32_t(flags) << what);
- ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled);
- setPowerStates(mEnabled);
- pthread_mutex_unlock(&mMplMutex);
- if (!newState)
- update_delay();
- return err;
- }
- pthread_mutex_unlock(&mMplMutex);
- return err;
-}
-
-int MPLSensor::setDelay(int32_t handle, int64_t ns)
-{
- FUNC_LOG;
- ALOGV_IF(EXTRA_VERBOSE,
- " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL));
- int what = -1;
- switch (handle) {
- case ID_A:
- what = Accelerometer;
- break;
- case ID_M:
- what = MagneticField;
- break;
- case ID_O:
- what = Orientation;
- break;
- case ID_GY:
- what = Gyro;
- break;
- case ID_GR:
- what = Gravity;
- break;
- case ID_RV:
- what = RotationVector;
- break;
- case ID_LA:
- what = LinearAccel;
- break;
- default:
- what = handle;
- break;
- }
-
- if (uint32_t(what) >= numSensors)
- return -EINVAL;
-
- if (ns < 0)
- return -EINVAL;
-
- pthread_mutex_lock(&mMplMutex);
- mDelays[what] = ns;
- pthread_mutex_unlock(&mMplMutex);
- return update_delay();
-}
-
-int MPLSensor::update_delay()
-{
- FUNC_LOG;
- int rv = 0;
- bool irq_set[5];
-
- pthread_mutex_lock(&mMplMutex);
-
- if (mEnabled) {
- uint64_t wanted = -1LLU;
- for (int i = 0; i < numSensors; i++) {
- if (mEnabled & (1 << i)) {
- uint64_t ns = mDelays[i];
- wanted = wanted < ns ? wanted : ns;
- }
- }
-
- //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns
- if (wanted < 10000000LLU) {
- wanted = 10000000LLU;
- }
-
- int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1
- : 0); //mpu fifo rate is in increments of 5ms
- if (rate == 0) //KLP disallow fifo rate 0
- rate = 1;
-
- if (rate != mCurFifoRate) {
- inv_error_t res; // = inv_dmp_stop();
- res = inv_set_fifo_rate(rate);
- ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
-
- mCurFifoRate = rate;
- rv = (res == INV_SUCCESS);
- }
-
- if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) {
- if (mUseTimerirq) {
- ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
- clearIrqData(irq_set);
- if (inv_get_dl_config()->requested_sensors
- == INV_THREE_AXIS_COMPASS) {
- ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
- (unsigned long) (wanted / 1000000LLU));
- ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
- (int) (wanted / 1000000LLU));
- } else {
- ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
- (unsigned long) inv_get_sample_step_size_ms());
- ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
- (int) inv_get_sample_step_size_ms());
- }
- }
- }
-
- }
- pthread_mutex_unlock(&mMplMutex);
- return rv;
-}
-
-/* return the current time in nanoseconds */
-int64_t MPLSensor::now_ns(void)
-{
- //FUNC_LOG;
- struct timespec ts;
-
- clock_gettime(CLOCK_MONOTONIC, &ts);
- return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
-}
-
-int MPLSensor::readEvents(sensors_event_t* data, int count)
-{
- //VFUNC_LOG;
- bool irq_set[5] = { false, false, false, false, false };
- inv_error_t rv;
- if (count < 1)
- return -EINVAL;
- int numEventReceived = 0;
-
- clearIrqData(irq_set);
-
- pthread_mutex_lock(&mMplMutex);
- if (mDmpStarted) {
- rv = inv_update_data();
- ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv);
- }
-
- else {
- //probably just one extra read after shutting down
- ALOGV_IF(EXTRA_VERBOSE,
- "MPLSensor::readEvents called, but there's nothing to do.");
- }
-
- pthread_mutex_unlock(&mMplMutex);
-
- if (!mNewData) {
- ALOGV_IF(EXTRA_VERBOSE, "no new data");
- return 0;
- }
- mNewData = 0;
-
- /* google timestamp */
- pthread_mutex_lock(&mMplMutex);
- for (int i = 0; i < numSensors; i++) {
- if (mEnabled & (1 << i)) {
- CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i,
- &mPendingMask, i);
- mPendingEvents[i].timestamp = irq_timestamp;
- }
- }
-
- for (int j = 0; count && mPendingMask && j < numSensors; j++) {
- if (mPendingMask & (1 << j)) {
- mPendingMask &= ~(1 << j);
- if (mEnabled & (1 << j)) {
- *data++ = mPendingEvents[j];
- count--;
- numEventReceived++;
- }
- }
-
- }
-
- pthread_mutex_unlock(&mMplMutex);
- return numEventReceived;
-}
-
-int MPLSensor::getFd() const
-{
- return data_fd;
-}
-
-int MPLSensor::getAccelFd() const
-{
- return accel_fd;
-}
-
-int MPLSensor::getTimerFd() const
-{
- return timer_fd;
-}
-
-int MPLSensor::getPowerFd() const
-{
- int hdl = (uintptr_t) inv_get_serial_handle();
- return hdl;
-}
-
-int MPLSensor::getPollTime()
-{
- return mPollTime;
-}
-
-bool MPLSensor::hasPendingEvents() const
-{
- //if we are using the polling workaround, force the main loop to check for data every time
- return (mPollTime != -1);
-}
-
-void MPLSensor::handlePowerEvent()
-{
- VFUNC_LOG;
- mpuirq_data irqd;
-
- int fd = (uintptr_t) inv_get_serial_handle();
- read(fd, &irqd, sizeof(irqd));
-
- if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) {
- //going to sleep
- sleepEvent();
- } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) {
- //waking up
- wakeEvent();
- }
-
- ioctl(fd, MPU_PM_EVENT_HANDLED, 0);
-}
-
-void MPLSensor::sleepEvent()
-{
- VFUNC_LOG;
- pthread_mutex_lock(&mMplMutex);
- if (mEnabled != 0) {
- mForceSleep = true;
- mOldEnabledMask = mEnabled;
- setPowerStates(0);
- }
- pthread_mutex_unlock(&mMplMutex);
-}
-
-void MPLSensor::wakeEvent()
-{
- VFUNC_LOG;
- pthread_mutex_lock(&mMplMutex);
- if (mForceSleep) {
- setPowerStates((mOldEnabledMask | mEnabled));
- }
- mForceSleep = false;
- pthread_mutex_unlock(&mMplMutex);
-}
-
-/** fill in the sensor list based on which sensors are configured.
- * return the number of configured sensors.
- * parameter list must point to a memory region of at least 7*sizeof(sensor_t)
- * parameter len gives the length of the buffer pointed to by list
- */
-
-int MPLSensor::populateSensorList(struct sensor_t *list, size_t len)
-{
- int numsensors;
-
- if(len < 7*sizeof(sensor_t)) {
- ALOGE("sensor list too small, not populating.");
- return 0;
- }
-
- /* fill in the base values */
- memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
-
- /* first add gyro, accel and compass to the list */
-
- /* fill in accel values */
- unsigned short accelId = inv_get_accel_id();
- if(accelId == 0) {
- ALOGE("Can not get accel id");
- }
- fillAccel(accelId, list);
-
- /* fill in compass values */
- unsigned short compassId = inv_get_compass_id();
- if(compassId == 0) {
- ALOGE("Can not get compass id");
- }
- fillCompass(compassId, list);
-
- /* fill in gyro values */
- fillGyro(MPU_NAME, list);
-
- if(mNineAxisEnabled)
- {
- numsensors = 7;
- /* all sensors will be added to the list */
- /* fill in orientation values */
- fillOrientation(list);
-
- /* fill in rotation vector values */
- fillRV(list);
-
- /* fill in gravity values */
- fillGravity(list);
-
- /* fill in Linear accel values */
- fillLinearAccel(list);
- } else {
- /* no 9-axis sensors, zero fill that part of the list */
- numsensors = 3;
- memset(list+3, 0, 4*sizeof(struct sensor_t));
- }
-
- return numsensors;
-}
-
-void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list)
-{
- switch (accel) {
- case ACCEL_ID_LIS331:
- list[Accelerometer].maxRange = ACCEL_LIS331_RANGE;
- list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION;
- list[Accelerometer].power = ACCEL_LIS331_POWER;
- break;
-
- case ACCEL_ID_LIS3DH:
- list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE;
- list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION;
- list[Accelerometer].power = ACCEL_LIS3DH_POWER;
- break;
-
- case ACCEL_ID_KXSD9:
- list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE;
- list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION;
- list[Accelerometer].power = ACCEL_KXSD9_POWER;
- break;
-
- case ACCEL_ID_KXTF9:
- list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE;
- list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION;
- list[Accelerometer].power = ACCEL_KXTF9_POWER;
- break;
-
- case ACCEL_ID_BMA150:
- list[Accelerometer].maxRange = ACCEL_BMA150_RANGE;
- list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION;
- list[Accelerometer].power = ACCEL_BMA150_POWER;
- break;
-
- case ACCEL_ID_BMA222:
- list[Accelerometer].maxRange = ACCEL_BMA222_RANGE;
- list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION;
- list[Accelerometer].power = ACCEL_BMA222_POWER;
- break;
-
- case ACCEL_ID_BMA250:
- list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
- list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
- list[Accelerometer].power = ACCEL_BMA250_POWER;
- break;
-
- case ACCEL_ID_ADXL34X:
- list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE;
- list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION;
- list[Accelerometer].power = ACCEL_ADXL34X_POWER;
- break;
-
- case ACCEL_ID_MMA8450:
- list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
- list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
- list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
- break;
-
- case ACCEL_ID_MMA845X:
- list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE;
- list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION;
- list[Accelerometer].power = ACCEL_MMA845X_POWER;
- break;
-
- case ACCEL_ID_MPU6050:
- list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
- list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
- list[Accelerometer].power = ACCEL_MPU6050_POWER;
- break;
- default:
- ALOGE("unknown accel id -- accel params will be wrong.");
- break;
- }
-}
-
-void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list)
-{
- switch (compass) {
- case COMPASS_ID_AK8975:
- list[MagneticField].maxRange = COMPASS_AKM8975_RANGE;
- list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION;
- list[MagneticField].power = COMPASS_AKM8975_POWER;
- break;
- case COMPASS_ID_AMI30X:
- list[MagneticField].maxRange = COMPASS_AMI30X_RANGE;
- list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION;
- list[MagneticField].power = COMPASS_AMI30X_POWER;
- break;
- case COMPASS_ID_AMI306:
- list[MagneticField].maxRange = COMPASS_AMI306_RANGE;
- list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
- list[MagneticField].power = COMPASS_AMI306_POWER;
- break;
- case COMPASS_ID_YAS529:
- list[MagneticField].maxRange = COMPASS_YAS529_RANGE;
- list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
- list[MagneticField].power = COMPASS_AMI306_POWER;
- break;
- case COMPASS_ID_YAS530:
- list[MagneticField].maxRange = COMPASS_YAS530_RANGE;
- list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION;
- list[MagneticField].power = COMPASS_YAS530_POWER;
- break;
- case COMPASS_ID_HMC5883:
- list[MagneticField].maxRange = COMPASS_HMC5883_RANGE;
- list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION;
- list[MagneticField].power = COMPASS_HMC5883_POWER;
- break;
- case COMPASS_ID_MMC314X:
- list[MagneticField].maxRange = COMPASS_MMC314X_RANGE;
- list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION;
- list[MagneticField].power = COMPASS_MMC314X_POWER;
- break;
- case COMPASS_ID_HSCDTD002B:
- list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE;
- list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION;
- list[MagneticField].power = COMPASS_HSCDTD002B_POWER;
- break;
- case COMPASS_ID_HSCDTD004A:
- list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE;
- list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION;
- list[MagneticField].power = COMPASS_HSCDTD004A_POWER;
- break;
- default:
- ALOGE("unknown compass id -- compass parameters will be wrong");
- }
-}
-
-void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
-{
- if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) {
- list[Gyro].maxRange = GYRO_MPU3050_RANGE;
- list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
- list[Gyro].power = GYRO_MPU3050_POWER;
- } else {
- list[Gyro].maxRange = GYRO_MPU6050_RANGE;
- list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
- list[Gyro].power = GYRO_MPU6050_POWER;
- }
- return;
-}
-
-
-/* fillRV depends on values of accel and compass in the list */
-void MPLSensor::fillRV(struct sensor_t *list)
-{
- /* compute power on the fly */
- list[RotationVector].power = list[Gyro].power + list[Accelerometer].power
- + list[MagneticField].power;
- list[RotationVector].resolution = .00001;
- list[RotationVector].maxRange = 1.0;
- return;
-}
-
-void MPLSensor::fillOrientation(struct sensor_t *list)
-{
- list[Orientation].power = list[Gyro].power + list[Accelerometer].power
- + list[MagneticField].power;
- list[Orientation].resolution = .00001;
- list[Orientation].maxRange = 360.0;
- return;
-}
-
-void MPLSensor::fillGravity( struct sensor_t *list)
-{
- list[Gravity].power = list[Gyro].power + list[Accelerometer].power
- + list[MagneticField].power;
- list[Gravity].resolution = .00001;
- list[Gravity].maxRange = 9.81;
- return;
-}
-
-void MPLSensor::fillLinearAccel(struct sensor_t *list)
-{
- list[Gravity].power = list[Gyro].power + list[Accelerometer].power
- + list[MagneticField].power;
- list[Gravity].resolution = list[Accelerometer].resolution;
- list[Gravity].maxRange = list[Accelerometer].maxRange;
- return;
-}
diff --git a/60xx/libsensors/MPLSensor.h b/60xx/libsensors/MPLSensor.h
deleted file mode 100644
index f580732..0000000
--- a/60xx/libsensors/MPLSensor.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * Copyright (C) 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.
- */
-/*************Removed the gesture related info for Google check in : Meenakshi Ramamoorthi: May 31st *********/
-
-#ifndef ANDROID_MPL_SENSOR_H
-#define ANDROID_MPL_SENSOR_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <poll.h>
-#include <utils/Vector.h>
-#include <utils/KeyedVector.h>
-#include "sensors.h"
-#include "SensorBase.h"
-
-/*****************************************************************************/
-/** MPLSensor implementation which fits into the HAL example for crespo provided
- * * by Google.
- * * WARNING: there may only be one instance of MPLSensor, ever.
- */
-
-class MPLSensor: public SensorBase
-{
- typedef void (MPLSensor::*hfunc_t)(sensors_event_t*, uint32_t*, int);
-
-public:
- MPLSensor();
- virtual ~MPLSensor();
-
- enum
- {
- Gyro=0,
- Accelerometer,
- MagneticField,
- Orientation,
- RotationVector,
- LinearAccel,
- Gravity,
- numSensors
- };
-
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int enable(int32_t handle, int enabled);
- virtual int readEvents(sensors_event_t *data, int count);
- virtual int getFd() const;
- virtual int getAccelFd() const;
- virtual int getTimerFd() const;
- virtual int getPowerFd() const;
- virtual int getPollTime();
- virtual bool hasPendingEvents() const;
- virtual void handlePowerEvent();
- virtual void sleepEvent();
- virtual void wakeEvent();
- int populateSensorList(struct sensor_t *list, size_t len);
- void cbOnMotion(uint16_t);
- void cbProcData();
-
-protected:
-
- void clearIrqData(bool* irq_set);
- void setPowerStates(int enabledsensor);
- void initMPL();
- void setupFIFO();
- void setupCallbacks();
- void gyroHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void accelHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void compassHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void rvHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void laHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void gravHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void orienHandler(sensors_event_t *data, uint32_t *pendmask, int index);
- void calcOrientationSensor(float *Rx, float *Val);
- int estimateCompassAccuracy();
-
- int mNewData; //flag indicating that the MPL calculated new output values
- int mDmpStarted;
- long mMasterSensorMask;
- long mLocalSensorMask;
- int mPollTime;
- int mCurFifoRate; //current fifo rate
- bool mHaveGoodMpuCal; //flag indicating that the cal file can be written
- bool mHaveGoodCompassCal;
- bool mUseTimerIrqAccel;
- bool mUsetimerIrqCompass;
- bool mUseTimerirq;
- struct pollfd mPollFds[4];
- int mSampleCount;
- pthread_mutex_t mMplMutex;
- int64_t now_ns();
- int64_t select_ns(unsigned long long time_set[]);
-
- enum FILEHANDLES
- {
- MPUIRQ_FD, ACCELIRQ_FD, COMPASSIRQ_FD, TIMERIRQ_FD,
- };
-
-private:
-
- int update_delay();
- int accel_fd;
- int timer_fd;
-
- uint32_t mEnabled;
- uint32_t mPendingMask;
- sensors_event_t mPendingEvents[numSensors];
- uint64_t mDelays[numSensors];
- hfunc_t mHandlers[numSensors];
- bool mForceSleep;
- long int mOldEnabledMask;
- android::KeyedVector<int, int> mIrqFds;
-
- /* added for dynamic get sensor list */
- bool mNineAxisEnabled;
- void fillAccel(unsigned char accel, struct sensor_t *list);
- void fillCompass(unsigned char compass, struct sensor_t *list);
- void fillGyro(const char* gyro, struct sensor_t *list);
- void fillRV(struct sensor_t *list);
- void fillOrientation(struct sensor_t *list);
- void fillGravity(struct sensor_t *list);
- void fillLinearAccel(struct sensor_t *list);
-};
-
-void setCallbackObject(MPLSensor*);
-
-/*****************************************************************************/
-
-#endif // ANDROID_MPL_SENSOR_H
diff --git a/60xx/libsensors/SensorBase.cpp b/60xx/libsensors/SensorBase.cpp
deleted file mode 100644
index 9cc1ee8..0000000
--- a/60xx/libsensors/SensorBase.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-/*
- * Copyright (C) 2008 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <string.h>
-#include <unistd.h>
-#include <dirent.h>
-#include <sys/select.h>
-
-#include <cutils/log.h>
-
-#include <linux/input.h>
-
-#include "SensorBase.h"
-
-/*****************************************************************************/
-
-SensorBase::SensorBase(
- const char* dev_name,
- const char* data_name)
- : dev_name(dev_name), data_name(data_name),
- dev_fd(-1), data_fd(-1)
-{
- if (data_name) {
- data_fd = openInput(data_name);
- }
-}
-
-SensorBase::~SensorBase() {
- if (data_fd >= 0) {
- close(data_fd);
- }
- if (dev_fd >= 0) {
- close(dev_fd);
- }
-}
-
-int SensorBase::open_device() {
- if (dev_fd<0 && dev_name) {
- dev_fd = open(dev_name, O_RDONLY);
- ALOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
- }
- return 0;
-}
-
-int SensorBase::close_device() {
- if (dev_fd >= 0) {
- close(dev_fd);
- dev_fd = -1;
- }
- return 0;
-}
-
-int SensorBase::getFd() const {
- if (!data_name) {
- return dev_fd;
- }
- return data_fd;
-}
-
-int SensorBase::setDelay(int32_t handle, int64_t ns) {
- return 0;
-}
-
-bool SensorBase::hasPendingEvents() const {
- return false;
-}
-
-int64_t SensorBase::getTimestamp() {
- struct timespec t;
- t.tv_sec = t.tv_nsec = 0;
- clock_gettime(CLOCK_MONOTONIC, &t);
- return int64_t(t.tv_sec)*1000000000LL + t.tv_nsec;
-}
-
-int SensorBase::openInput(const char* inputName) {
- int fd = -1;
- const char *dirname = "/dev/input";
- char devname[PATH_MAX];
- char *filename;
- DIR *dir;
- struct dirent *de;
- dir = opendir(dirname);
- if(dir == NULL)
- return -1;
- strcpy(devname, dirname);
- filename = devname + strlen(devname);
- *filename++ = '/';
- while((de = readdir(dir))) {
- if(de->d_name[0] == '.' &&
- (de->d_name[1] == '\0' ||
- (de->d_name[1] == '.' && de->d_name[2] == '\0')))
- continue;
- strcpy(filename, de->d_name);
- fd = open(devname, O_RDONLY);
- if (fd>=0) {
- char name[80];
- if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
- name[0] = '\0';
- }
- if (!strcmp(name, inputName)) {
- strcpy(input_name, filename);
- break;
- } else {
- close(fd);
- fd = -1;
- }
- }
- }
- closedir(dir);
- ALOGE_IF(fd<0, "couldn't find '%s' input device", inputName);
- return fd;
-}
diff --git a/60xx/libsensors/SensorBase.h b/60xx/libsensors/SensorBase.h
deleted file mode 100644
index bb4d055..0000000
--- a/60xx/libsensors/SensorBase.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * Copyright (C) 2008 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef ANDROID_SENSOR_BASE_H
-#define ANDROID_SENSOR_BASE_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-
-/*****************************************************************************/
-
-struct sensors_event_t;
-
-class SensorBase {
-protected:
- const char* dev_name;
- const char* data_name;
- char input_name[PATH_MAX];
- int dev_fd;
- int data_fd;
-
- int openInput(const char* inputName);
- static int64_t getTimestamp();
-
-
- static int64_t timevalToNano(timeval const& t) {
- return t.tv_sec*1000000000LL + t.tv_usec*1000;
- }
-
- int open_device();
- int close_device();
-
-public:
- SensorBase(
- const char* dev_name,
- const char* data_name);
-
- virtual ~SensorBase();
-
- virtual int readEvents(sensors_event_t* data, int count) = 0;
- virtual bool hasPendingEvents() const;
- virtual int getFd() const;
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int enable(int32_t handle, int enabled) = 0;
-};
-
-/*****************************************************************************/
-
-#endif // ANDROID_SENSOR_BASE_H
diff --git a/60xx/libsensors/sensor_params.h b/60xx/libsensors/sensor_params.h
deleted file mode 100644
index 4925ac4..0000000
--- a/60xx/libsensors/sensor_params.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * Copyright (C) 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.
- */
-
-#ifndef INV_SENSOR_PARAMS_H
-#define INV_SENSOR_PARAMS_H
-
-/* Physical parameters of the sensors supported by Invensense MPL */
-#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV)
-#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA)
-#define SENSORS_GRAVITY_HANDLE (ID_GR)
-#define SENSORS_GYROSCOPE_HANDLE (ID_GY)
-#define SENSORS_ACCELERATION_HANDLE (ID_A)
-#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M)
-#define SENSORS_ORIENTATION_HANDLE (ID_O)
-/******************************************/
-//COMPASS_ID_AKM
-#define COMPASS_AKM8975_RANGE (9830.0f)
-#define COMPASS_AKM8975_RESOLUTION (0.285f)
-#define COMPASS_AKM8975_POWER (10.0f)
-//COMPASS_ID_AMI30X
-#define COMPASS_AMI30X_RANGE (5461.0f)
-#define COMPASS_AMI30X_RESOLUTION (0.9f)
-#define COMPASS_AMI30X_POWER (0.15f)
-//COMPASS_ID_AMI306
-#define COMPASS_AMI306_RANGE (5461.0f)
-#define COMPASS_AMI306_RESOLUTION (0.9f)
-#define COMPASS_AMI306_POWER (0.15f)
-//COMPASS_ID_YAS529
-#define COMPASS_YAS529_RANGE (19660.0f)
-#define COMPASS_YAS529_RESOLUTION (0.012f)
-#define COMPASS_YAS529_POWER (4.0f)
-//COMPASS_ID_YAS530
-#define COMPASS_YAS530_RANGE (8001.0f)
-#define COMPASS_YAS530_RESOLUTION (0.012f)
-#define COMPASS_YAS530_POWER (4.0f)
-//COMPASS_ID_HMC5883
-#define COMPASS_HMC5883_RANGE (10673.0f)
-#define COMPASS_HMC5883_RESOLUTION (10.0f)
-#define COMPASS_HMC5883_POWER (0.24f)
-//COMPASS_ID_LSM303DLH
-#define COMPASS_LSM303DLH_RANGE (10240.0f)
-#define COMPASS_LSM303DLH_RESOLUTION (1.0f)
-#define COMPASS_LSM303DLH_POWER (1.0f)
-//COMPASS_ID_LSM303DLM
-#define COMPASS_LSM303DLM_RANGE (10240.0f)
-#define COMPASS_LSM303DLM_RESOLUTION (1.0f)
-#define COMPASS_LSM303DLM_POWER (1.0f)
-//COMPASS_ID_MMC314X
-#define COMPASS_MMC314X_RANGE (400.0f)
-#define COMPASS_MMC314X_RESOLUTION (2.0f)
-#define COMPASS_MMC314X_POWER (0.55f)
-//COMPASS_ID_HSCDTD002B
-#define COMPASS_HSCDTD002B_RANGE (9830.0f)
-#define COMPASS_HSCDTD002B_RESOLUTION (1.0f)
-#define COMPASS_HSCDTD002B_POWER (1.0f)
-//COMPASS_ID_HSCDTD004A
-#define COMPASS_HSCDTD004A_RANGE (9830.0f)
-#define COMPASS_HSCDTD004A_RESOLUTION (1.0f)
-#define COMPASS_HSCDTD004A_POWER (1.0f)
-/*******************************************/
-//ACCEL_ID_LIS331
-#define ACCEL_LIS331_RANGE (2.480f*GRAVITY_EARTH)
-#define ACCEL_LIS331_RESOLUTION (.001f*GRAVITY_EARTH)
-#define ACCEL_LIS331_POWER (1.0f)
-//ACCEL_ID_LSM303DLX
-#define ACCEL_LSM303DLX_RANGE (2.480f*GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_POWER (1.0f)
-//ACCEL_ID_LIS3DH
-#define ACCEL_LIS3DH_RANGE (2.480f*GRAVITY_EARTH)
-#define ACCEL_LIS3DH_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_LIS3DH_POWER (1.0f)
-//ACCEL_ID_KXSD9
-#define ACCEL_KXSD9_RANGE (2.5006f*GRAVITY_EARTH)
-#define ACCEL_KXSD9_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_KXSD9_POWER (1.0f)
-//ACCEL_ID_KXTF9
-#define ACCEL_KXTF9_RANGE (1.0f*GRAVITY_EARTH)
-#define ACCEL_KXTF9_RESOLUTION (0.033f*GRAVITY_EARTH)
-#define ACCEL_KXTF9_POWER (0.35f)
-//ACCEL_ID_BMA150
-#define ACCEL_BMA150_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_BMA150_RESOLUTION (0.004f*GRAVITY_EARTH)
-#define ACCEL_BMA150_POWER (0.2f)
-//ACCEL_ID_BMA222
-#define ACCEL_BMA222_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_BMA222_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_BMA222_POWER (0.1f)
-//ACCEL_ID_BMA250
-#define ACCEL_BMA250_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_BMA250_RESOLUTION (0.00391f*GRAVITY_EARTH)
-#define ACCEL_BMA250_POWER (0.139f)
-//ACCEL_ID_ADXL34X
-#define ACCEL_ADXL34X_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_ADXL34X_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_ADXL34X_POWER (1.0f)
-//ACCEL_ID_MMA8450
-#define ACCEL_MMA8450_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_MMA8450_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_MMA8450_POWER (1.0f)
-//ACCEL_ID_MMA845X
-#define ACCEL_MMA845X_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_MMA845X_RESOLUTION (0.001f*GRAVITY_EARTH)
-#define ACCEL_MMA845X_POWER (1.0f)
-//ACCEL_ID_MPU6050
-#define ACCEL_MPU6050_RANGE (2.0f*GRAVITY_EARTH)
-#define ACCEL_MPU6050_RESOLUTION (0.004f*GRAVITY_EARTH)
-#define ACCEL_MPU6050_POWER (0.0f)
-/******************************************/
-//GYRO MPU3050
-#define RAD_P_DEG (3.14159f/180.0f)
-#define GYRO_MPU3050_RANGE (2000.0f*RAD_P_DEG)
-#define GYRO_MPU3050_RESOLUTION (32.8f*RAD_P_DEG)
-#define GYRO_MPU3050_POWER (6.1f)
-//GYRO MPU6050
-#define GYRO_MPU6050_RANGE (2000.0f*RAD_P_DEG)
-#define GYRO_MPU6050_RESOLUTION (16.4f*RAD_P_DEG)
-#define GYRO_MPU6050_POWER (5.5f)
-
-#endif
-
diff --git a/60xx/libsensors/sensors.h b/60xx/libsensors/sensors.h
deleted file mode 100644
index 5c56da0..0000000
--- a/60xx/libsensors/sensors.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * Copyright (C) 2008 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef ANDROID_SENSORS_H
-#define ANDROID_SENSORS_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <hardware/hardware.h>
-#include <hardware/sensors.h>
-
-__BEGIN_DECLS
-
-/*****************************************************************************/
-
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
-
-#define ID_MPL_BASE (0)
-#define ID_GY (ID_MPL_BASE)
-#define ID_A (ID_GY + 1)
-#define ID_M (ID_A + 1)
-#define ID_O (ID_M + 1)
-#define ID_RV (ID_O + 1)
-#define ID_LA (ID_RV + 1)
-#define ID_GR (ID_LA + 1)
-
-/*****************************************************************************/
-
-/*
- * The SENSORS Module
- */
-
-__END_DECLS
-
-#endif // ANDROID_SENSORS_H
diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk
deleted file mode 100644
index fd8e472..0000000
--- a/60xx/libsensors_iio/Android.mk
+++ /dev/null
@@ -1,118 +0,0 @@
-# Copyright (C) 2008 The Android Open Source Project
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-# Modified 2011 by InvenSense, Inc
-
-LOCAL_PATH := $(call my-dir)
-
-# InvenSense fragment of the HAL
-include $(CLEAR_VARS)
-
-LOCAL_MODULE := libinvensense_hal
-
-LOCAL_MODULE_TAGS := optional
-LOCAL_MODULE_OWNER := invensense
-
-LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall
-# Comment out for ICS. Affects Android LOG macros.
-LOCAL_CFLAGS += -DANDROID_JELLYBEAN
-
-ifeq ($(ENG_BUILD),1)
-ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1)
-LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
-endif
-ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1)
-LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL
-endif
-ifeq ($(COMPILE_COMPASS_YAS53x),1)
-LOCAL_CFLAGS += -DCOMPASS_YAS53x
-endif
-ifeq ($(COMPILE_COMPASS_AK8975),1)
-LOCAL_CFLAGS += -DCOMPASS_AK8975
-endif
-ifeq ($(COMPILE_COMPASS_AMI306),1)
-LOCAL_CFLAGS += -DCOMPASS_AMI306
-endif
-else # release builds, default
-LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL
-endif
-
-LOCAL_SRC_FILES := SensorBase.cpp
-LOCAL_SRC_FILES += MPLSensor.cpp
-LOCAL_SRC_FILES += MPLSupport.cpp
-LOCAL_SRC_FILES += InputEventReader.cpp
-LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp
-
-LOCAL_C_INCLUDES += $(LOCAL_PATH)
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
-
-LOCAL_SHARED_LIBRARIES := liblog
-LOCAL_SHARED_LIBRARIES += libcutils
-LOCAL_SHARED_LIBRARIES += libutils
-LOCAL_SHARED_LIBRARIES += libdl
-LOCAL_SHARED_LIBRARIES += libmllite
-
-# Additions for SysPed
-LOCAL_SHARED_LIBRARIES += libmplmpu
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl
-LOCAL_CPPFLAGS += -DLINUX=1
-# Experimental
-ifeq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true)
-LOCAL_CPPFLAGS += -DLIBMLLITE_FROM_SOURCE
-endif
-
-include $(BUILD_SHARED_LIBRARY)
-
-include $(CLEAR_VARS)
-LOCAL_MODULE := libmplmpu
-LOCAL_SRC_FILES := libmplmpu.so
-LOCAL_MODULE_TAGS := optional
-LOCAL_MODULE_OWNER := invensense
-LOCAL_MODULE_SUFFIX := .so
-LOCAL_MODULE_CLASS := SHARED_LIBRARIES
-LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
-OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
-LOCAL_STRIP_MODULE := true
-include $(BUILD_PREBUILT)
-
-# Experimental
-ifneq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true)
-include $(CLEAR_VARS)
-LOCAL_MODULE := libmllite
-LOCAL_SRC_FILES := libmllite.so
-LOCAL_MODULE_TAGS := optional
-LOCAL_MODULE_OWNER := invensense
-LOCAL_MODULE_SUFFIX := .so
-LOCAL_MODULE_CLASS := SHARED_LIBRARIES
-LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
-OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES)
-LOCAL_STRIP_MODULE := true
-include $(BUILD_PREBUILT)
-else
-include $(CLEAR_VARS)
-LOCAL_CFLAGS += -DANDROID_JELLYBEAN
-LOCAL_CFLAGS += -DLINUX=1
-LOCAL_MODULE := libmllite
-LOCAL_SRC_FILES := $(call all-c-files-under, software/core/mllite)
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include
-LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux
-LOCAL_MODULE_OWNER := invensense
-LOCAL_MODULE_PATH := $(TARGET_OUT)/lib
-LOCAL_SHARED_LIBRARIES := liblog
-include $(BUILD_SHARED_LIBRARY)
-endif
diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
deleted file mode 100644
index be8c912..0000000
--- a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp
+++ /dev/null
@@ -1,390 +0,0 @@
-/*
- * Copyright (C) 2012 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#define LOG_NDEBUG 0
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <unistd.h>
-#include <dirent.h>
-#include <sys/select.h>
-#include <cutils/log.h>
-#include <linux/input.h>
-#include <string.h>
-
-#include "CompassSensor.IIO.9150.h"
-#include "sensors.h"
-#include "MPLSupport.h"
-#include "sensor_params.h"
-#include "ml_sysfs_helper.h"
-#include "local_log_def.h"
-
-#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*)
-
-#if defined COMPASS_YAS53x
-# pragma message "Invensense compass cal with YAS53x IIO on secondary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "INV_YAS530"
-#elif defined COMPASS_AK8975
-# pragma message "Invensense compass cal with AK8975 on primary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "INV_AK8975"
-#elif defined INVENSENSE_COMPASS_CAL
-# pragma message "Invensense compass cal with compass IIO on secondary bus"
-# define USE_MPL_COMPASS_HAL (1)
-# define COMPASS_NAME "INV_COMPASS"
-#else
-# pragma message "third party compass cal HAL"
-# define USE_MPL_COMPASS_HAL (0)
-// TODO: change to vendor's name
-# define COMPASS_NAME "AKM8975"
-#endif
-
-
-/*****************************************************************************/
-
-CompassSensor::CompassSensor()
- : SensorBase(NULL, NULL),
- compass_fd(-1),
- mCompassTimestamp(0),
- mCompassInputReader(8)
-{
- VFUNC_LOG;
-
- if(inv_init_sysfs_attributes()) {
- LOGE("Error Instantiating Compass\n");
- return;
- }
-
- if (!strcmp(COMPASS_NAME, "INV_COMPASS")) {
- mI2CBus = COMPASS_BUS_SECONDARY;
- } else {
- mI2CBus = COMPASS_BUS_PRIMARY;
- }
-
- memset(mCachedCompassData, 0, sizeof(mCachedCompassData));
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- compassSysFs.compass_orient, getTimestamp());
- FILE *fptr;
- fptr = fopen(compassSysFs.compass_orient, "r");
- if (fptr != NULL) {
- int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
- &om[6], &om[7], &om[8]);
- fclose(fptr);
-
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:compass mounting matrix: "
- "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
- om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
-
- mCompassOrientation[0] = om[0];
- mCompassOrientation[1] = om[1];
- mCompassOrientation[2] = om[2];
- mCompassOrientation[3] = om[3];
- mCompassOrientation[4] = om[4];
- mCompassOrientation[5] = om[5];
- mCompassOrientation[6] = om[6];
- mCompassOrientation[7] = om[7];
- mCompassOrientation[8] = om[8];
- } else {
- LOGE("HAL:Couldn't read compass mounting matrix");
- }
-
- if (!isIntegrated()) {
- enable(ID_M, 0);
- }
-}
-
-CompassSensor::~CompassSensor()
-{
- VFUNC_LOG;
-
- free(pathP);
- if( compass_fd > 0)
- close(compass_fd);
-}
-
-int CompassSensor::getFd() const
-{
- VHANDLER_LOG;
- return compass_fd;
-}
-
-/**
- * @brief This function will enable/disable sensor.
- * @param[in] handle
- * which sensor to enable/disable.
- * @param[in] en
- * en=1, enable;
- * en=0, disable
- * @return if the operation is successful.
- */
-int CompassSensor::enable(int32_t /*handle*/, int en)
-{
- VFUNC_LOG;
-
- mEnable = en;
- int res;
-
- res = write_sysfs_int(compassSysFs.compass_enable, en);
- LOGE_IF(res < 0, "HAL:enable compass failed");
-
- if (en) {
- res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en);
- res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en);
- res = write_sysfs_int(compassSysFs.compass_z_fifo_enable, en);
- }
-
- return res;
-}
-
-int CompassSensor::setDelay(int32_t /*handle*/, int64_t ns)
-{
- VFUNC_LOG;
- int tempFd;
- int res;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp());
- mDelay = ns;
- if (ns == 0)
- return -1;
- tempFd = open(compassSysFs.compass_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, 1000000000.f / ns);
- if(res < 0) {
- LOGE("HAL:Compass update delay error");
- }
- return res;
-}
-
-
-/**
- @brief This function will return the state of the sensor.
- @return 1=enabled; 0=disabled
-**/
-int CompassSensor::getEnable(int32_t /*handle*/)
-{
- VFUNC_LOG;
- return mEnable;
-}
-
-/* use for Invensense compass calibration */
-#define COMPASS_EVENT_DEBUG (0)
-void CompassSensor::processCompassEvent(const input_event *event)
-{
- VHANDLER_LOG;
-
- switch (event->code) {
- case EVENT_TYPE_ICOMPASS_X:
- LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n");
- mCachedCompassData[0] = event->value;
- break;
- case EVENT_TYPE_ICOMPASS_Y:
- LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n");
- mCachedCompassData[1] = event->value;
- break;
- case EVENT_TYPE_ICOMPASS_Z:
- LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n");
- mCachedCompassData[2] = event->value;
- break;
- }
-
- mCompassTimestamp =
- (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L;
-}
-
-void CompassSensor::getOrientationMatrix(signed char *orient)
-{
- VFUNC_LOG;
- memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation));
-}
-
-long CompassSensor::getSensitivity()
-{
- VFUNC_LOG;
-
- long sensitivity;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- compassSysFs.compass_scale, getTimestamp());
- inv_read_data(compassSysFs.compass_scale, &sensitivity);
- return sensitivity;
-}
-
-/**
- @brief This function is called by sensors_mpl.cpp
- to read sensor data from the driver.
- @param[out] data sensor data is stored in this variable. Scaled such that
- 1 uT = 2^16
- @para[in] timestamp data's timestamp
- @return 1, if 1 sample read, 0, if not, negative if error
- */
-int CompassSensor::readSample(long *data, int64_t *timestamp)
-{
- VHANDLER_LOG;
-
- int done = 0;
-
- ssize_t n = mCompassInputReader.fill(compass_fd);
- if (n < 0) {
- LOGE("HAL:no compass events read");
- return n;
- }
-
- input_event const* event;
-
- while (done == 0 && mCompassInputReader.readEvent(&event)) {
- int type = event->type;
- if (type == EV_REL) {
- processCompassEvent(event);
- } else if (type == EV_SYN) {
- *timestamp = mCompassTimestamp;
- memcpy(data, mCachedCompassData, sizeof(mCachedCompassData));
- done = 1;
- } else {
- LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)",
- type, event->code);
- }
- mCompassInputReader.next();
- }
-
- return done;
-}
-
-/**
- * @brief This function will return the current delay for this sensor.
- * @return delay in nanoseconds.
- */
-int64_t CompassSensor::getDelay(int32_t /*handle*/)
-{
- VFUNC_LOG;
- return mDelay;
-}
-
-void CompassSensor::fillList(struct sensor_t *list)
-{
- VFUNC_LOG;
-
- const char *compass = COMPASS_NAME;
-
- if (compass) {
- if(!strcmp(compass, "INV_COMPASS")) {
- list->maxRange = COMPASS_MPU9150_RANGE;
- /* since target platform would use AK8963 instead of AK8975,
- need to adopt AK8963's resolution here */
- list->resolution = COMPASS_AKM8963_RESOLUTION;
- list->power = COMPASS_MPU9150_POWER;
- list->minDelay = COMPASS_MPU9150_MINDELAY;
- return;
- }
- if(!strcmp(compass, "compass")
- || !strcmp(compass, "INV_AK8975") ) {
- list->maxRange = COMPASS_AKM8975_RANGE;
- list->resolution = COMPASS_AKM8975_RESOLUTION;
- list->power = COMPASS_AKM8975_POWER;
- list->minDelay = COMPASS_AKM8975_MINDELAY;
- return;
- }
- if(!strcmp(compass, "INV_YAS530")) {
- list->maxRange = COMPASS_YAS53x_RANGE;
- list->resolution = COMPASS_YAS53x_RESOLUTION;
- list->power = COMPASS_YAS53x_POWER;
- list->minDelay = COMPASS_YAS53x_MINDELAY;
- return;
- }
- if(!strcmp(compass, "INV_AMI306")) {
- list->maxRange = COMPASS_AMI306_RANGE;
- list->resolution = COMPASS_AMI306_RESOLUTION;
- list->power = COMPASS_AMI306_POWER;
- list->minDelay = COMPASS_AMI306_MINDELAY;
- return;
- }
- }
-
- LOGE("HAL:unknown compass id %s -- "
- "params default to ak8975 and might be wrong.",
- compass);
- list->maxRange = COMPASS_AKM8975_RANGE;
- list->resolution = COMPASS_AKM8975_RESOLUTION;
- list->power = COMPASS_AKM8975_POWER;
- list->minDelay = COMPASS_AKM8975_MINDELAY;
-}
-
-int CompassSensor::inv_init_sysfs_attributes(void)
-{
- VFUNC_LOG;
-
- unsigned char i = 0;
- char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
- char *sptr;
- char **dptr;
-
- pathP = (char*)malloc(
- sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
- sptr = pathP;
- dptr = (char**)&compassSysFs;
- if (sptr == NULL)
- return -1;
-
- do {
- *dptr++ = sptr;
- sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
- } while (++i < COMPASS_MAX_SYSFS_ATTRB);
-
- // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
- // inv_get_sysfs_abs_path(sysfs_path);
- if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
- ALOGE("CompassSensor failed get sysfs path");
- return -1;
- }
-
- inv_get_iio_trigger_path(iio_trigger_path);
-
-#if defined COMPASS_AK8975
- char tbuf[2];
- int num;
- inv_get_input_number(COMPASS_NAME, &num);
- tbuf[0] = num + 0x30;
- tbuf[1] = 0;
- sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf);
- strcat(sysfs_path, "/ak8975");
-
- sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable");
- sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate");
- sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale");
- sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
-#else
- sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable");
- sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en");
- sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en");
- sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en");
- sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency");
- sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale");
- sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix");
-#endif
-
-#if SYSFS_VERBOSE
- // test print sysfs paths
- dptr = (char**)&compassSysFs;
- for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) {
- LOGE("HAL:sysfs path: %s", *dptr++);
- }
-#endif
- return 0;
-}
diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.h b/60xx/libsensors_iio/CompassSensor.IIO.9150.h
deleted file mode 100644
index 6e9bf03..0000000
--- a/60xx/libsensors_iio/CompassSensor.IIO.9150.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * Copyright (C) 2012 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef COMPASS_SENSOR_H
-#define COMPASS_SENSOR_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-// TODO fixme, need input_event
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <poll.h>
-#include <utils/Vector.h>
-#include <utils/KeyedVector.h>
-
-#include "sensors.h"
-#include "SensorBase.h"
-#include "InputEventReader.h"
-
-class CompassSensor : public SensorBase {
-
-public:
- CompassSensor();
- virtual ~CompassSensor();
-
- virtual int getFd() const;
- virtual int enable(int32_t handle, int enabled);
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int getEnable(int32_t handle);
- virtual int64_t getDelay(int32_t handle);
-
- // unnecessary for MPL
- virtual int readEvents(sensors_event_t* /*data*/, int /*count*/) { return 0; }
-
- int readSample(long *data, int64_t *timestamp);
- int providesCalibration() { return 0; }
- void getOrientationMatrix(signed char *orient);
- long getSensitivity();
- int getAccuracy() { return 0; }
- void fillList(struct sensor_t *list);
- int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); }
-
-private:
- enum CompassBus {
- COMPASS_BUS_PRIMARY = 0,
- COMPASS_BUS_SECONDARY = 1
- } mI2CBus;
-
- struct sysfs_attrbs {
- char *compass_enable;
- char *compass_x_fifo_enable;
- char *compass_y_fifo_enable;
- char *compass_z_fifo_enable;
- char *compass_rate;
- char *compass_scale;
- char *compass_orient;
- } compassSysFs;
-
- // implementation specific
- signed char mCompassOrientation[9];
- long mCachedCompassData[3];
- int compass_fd;
- int64_t mCompassTimestamp;
- InputEventCircularReader mCompassInputReader;
- int64_t mDelay;
- int mEnable;
- char *pathP;
-
- void processCompassEvent(const input_event *event);
- int inv_init_sysfs_attributes(void);
-};
-
-/*****************************************************************************/
-
-#endif // COMPASS_SENSOR_H
diff --git a/60xx/libsensors_iio/InputEventReader.cpp b/60xx/libsensors_iio/InputEventReader.cpp
deleted file mode 100644
index fc48892..0000000
--- a/60xx/libsensors_iio/InputEventReader.cpp
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define LOG_NDEBUG 0
-
-#include <stdint.h>
-#include <errno.h>
-#include <string.h>
-#include <unistd.h>
-#include <poll.h>
-
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <cutils/log.h>
-
-#include "InputEventReader.h"
-#include "local_log_def.h"
-
-/*****************************************************************************/
-
-struct input_event;
-
-InputEventCircularReader::InputEventCircularReader(size_t numEvents)
- : mBuffer(new input_event[numEvents * 2]),
- mBufferEnd(mBuffer + numEvents),
- mHead(mBuffer),
- mCurr(mBuffer),
- mFreeSpace(numEvents)
-{
-}
-
-InputEventCircularReader::~InputEventCircularReader()
-{
- delete [] mBuffer;
-}
-
-#define INPUT_EVENT_DEBUG (0)
-ssize_t InputEventCircularReader::fill(int fd)
-{
- size_t numEventsRead = 0;
- LOGV_IF(INPUT_EVENT_DEBUG,
- "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
- if (mFreeSpace) {
- const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
- if (nread < 0 || nread % sizeof(input_event)) {
- //LOGE("Partial event received nread=%d, required=%d",
- // nread, sizeof(input_event));
- //LOGE("FD trying to read is: %d");
- // we got a partial event!!
- if (INPUT_EVENT_DEBUG) {
- LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n",
- __PRETTY_FUNCTION__);
- LOGV_IF(nread % sizeof(input_event),
- "DEBUG:%s exit nread %% sizeof(input_event)\n",
- __PRETTY_FUNCTION__);
- }
- return (nread < 0 ? -errno : -EINVAL);
- }
-
- numEventsRead = nread / sizeof(input_event);
- if (numEventsRead) {
- mHead += numEventsRead;
- mFreeSpace -= numEventsRead;
- if (mHead > mBufferEnd) {
- size_t s = mHead - mBufferEnd;
- memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
- mHead = mBuffer + s;
- }
- }
- }
-
- LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);
- return numEventsRead;
-}
-
-ssize_t InputEventCircularReader::readEvent(input_event const** events)
-{
- *events = mCurr;
- ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
- return available ? 1 : 0;
-}
-
-void InputEventCircularReader::next()
-{
- mCurr++;
- mFreeSpace++;
- if (mCurr >= mBufferEnd) {
- mCurr = mBuffer;
- }
-}
diff --git a/60xx/libsensors_iio/InputEventReader.h b/60xx/libsensors_iio/InputEventReader.h
deleted file mode 100644
index 5c752db..0000000
--- a/60xx/libsensors_iio/InputEventReader.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_INPUT_EVENT_READER_H
-#define ANDROID_INPUT_EVENT_READER_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include "SensorBase.h"
-
-/*****************************************************************************/
-
-struct input_event;
-
-class InputEventCircularReader
-{
- struct input_event* const mBuffer;
- struct input_event* const mBufferEnd;
- struct input_event* mHead;
- struct input_event* mCurr;
- ssize_t mFreeSpace;
-
-public:
- InputEventCircularReader(size_t numEvents);
- ~InputEventCircularReader();
- ssize_t fill(int fd);
- ssize_t readEvent(input_event const** events);
- void next();
-};
-
-/*****************************************************************************/
-
-#endif // ANDROID_INPUT_EVENT_READER_H
diff --git a/60xx/libsensors_iio/License.txt b/60xx/libsensors_iio/License.txt
deleted file mode 100644
index 930f931..0000000
--- a/60xx/libsensors_iio/License.txt
+++ /dev/null
@@ -1,217 +0,0 @@
-SOFTWARE LICENSE AGREEMENT
-
-Unless you and InvenSense Corporation ("InvenSense") execute a separate written
-software license agreement governing use of the accompanying software, this
-software is licensed to you under the terms of this Software License
-Agreement ("Agreement").
-
-ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR
-ACCEPTANCE OF THIS AGREEMENT.
-
-1. DEFINITIONS.
-
-1.1. "InvenSense Product" means any of the proprietary integrated circuit
-product(s) sold by InvenSense with which the Software was designed to be used,
-or their successors.
-
-1.2. "Licensee" means you or if you are accepting on behalf of an entity
-then the entity and its affiliates exercising rights under, and complying
-with all of the terms of this Agreement.
-
-1.3. "Software" shall mean that software made available by InvenSense to
-Licensee in binary code form with this Agreement.
-
-2. LICENSE GRANT; OWNERSHIP
-
-2.1. License Grants. Subject to the terms and conditions of this Agreement,
-InvenSense hereby grants to Licensee a non-exclusive, non-transferable,
-royalty-free license (i) to use and integrate the Software in conjunction
-with any other software; and (ii) to reproduce and distribute the Software
-complete, unmodified and only for use with a InvenSense Product.
-
-2.2. Restriction on Modification. If and to the extent that the Software is
-designed to be compliant with any published communications standard
-(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards),
-Licensee may not make any modifications to the Software that would cause the
-Software or the accompanying InvenSense Products to be incompatible with such
-standard.
-
-2.3. Restriction on Distribution. Licensee shall only distribute the
-Software (a) under the terms of this Agreement and a copy of this Agreement
-accompanies such distribution, and (b) agrees to defend and indemnify
-InvenSense and its licensors from and against any damages, costs, liabilities,
-settlement amounts and/or expenses (including attorneys' fees) incurred in
-connection with any claim, lawsuit or action by any third party that arises
-or results from the use or distribution of any and all Software by the
-Licensee except as contemplated herein.
-
-2.4. Proprietary Notices. Licensee shall not remove, efface or obscure any
-copyright or trademark notices from the Software. Licensee shall include
-reproductions of the InvenSense copyright notice with each copy of the
-Software, except where such Software is embedded in a manner not readily
-accessible to the end user. Licensee acknowledges that any symbols,
-trademarks, tradenames, and service marks adopted by InvenSense to identify the
-Software belong to InvenSense and that Licensee shall have no rights therein.
-
-2.5. Ownership. InvenSense shall retain all right, title and interest,
-including all intellectual property rights, in and to the Software. Licensee
-hereby covenants that it will not assert any claim that the Software created
-by or for InvenSense infringe any intellectual property right owned or
-controlled by Licensee.
-
-2.6. No Other Rights Granted; Restrictions. Apart from the license rights
-expressly set forth in this Agreement, InvenSense does not grant and Licensee
-does not receive any ownership right, title or interest nor any security
-interest or other interest in any intellectual property rights relating to
-the Software, nor in any copy of any part of the foregoing. No license is
-granted to Licensee in any human readable code of the Software (source code).
-Licensee shall not (i) use, license, sell or otherwise distribute the
-Software except as provided in this Agreement, (ii) attempt to reverse
-engineer, decompile or disassemble any portion of the Software; or (iii) use
-the Software or other material in violation of any applicable law or
-regulation, including but not limited to any regulatory agency, such as FCC,
-rules.
-
-3. NO WARRANTY OR SUPPORT
-
-3.1. No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND
-LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE,
-COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE. INVENSENSE SPECIFICALLY
-DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC
-PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR
-DOCUMENTATION FOR THE SOFTWARE. WITHOUT LIMITATION OF THE ABOVE, INVENSENSE
-GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT
-INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS
-THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR
-RELIABILITY.
-
-3.2. No Support. Nothing in this agreement shall obligate InvenSense to
-provide any support for the Software. InvenSense may, but shall be under no
-obligation to, correct any defects in the Software and/or provide updates to
-licensees of the Software. Licensee shall make reasonable efforts to
-promptly report to InvenSense any defects it finds in the Software, as an aid
-to creating improved revisions of the Software.
-
-3.3. Dangerous Applications. The Software is not designed, intended, or
-certified for use in components of systems intended for the operation of
-weapons, weapons systems, nuclear installations, means of mass
-transportation, aviation, life-support computers or equipment (including
-resuscitation equipment and surgical implants), pollution control, hazardous
-substances management, or for any other dangerous application in which the
-failure of the Software could create a situation where personal injury or
-death may occur. Licensee understands that use of the Software in such
-applications is fully at the risk of Licensee.
-
-4. TERM AND TERMINATION
-
-4.1. Termination. This Agreement will automatically terminate if Licensee
-fails to comply with any of the terms and conditions hereof. In such event,
-Licensee must destroy all copies of the Software and all of its component
-parts.
-
-4.2. Effect Of Termination. Upon any termination of this Agreement, the
-rights and licenses granted to Licensee under this Agreement shall
-immediately terminate.
-
-4.3. Survival. The rights and obligations under this Agreement which by
-their nature should survive termination will remain in effect after
-expiration or termination of this Agreement.
-
-5. CONFIDENTIALITY
-
-5.1. Obligations. Licensee acknowledges and agrees that any documentation
-relating to the Software, and any other information (if such other
-information is identified as confidential or should be recognized as
-confidential under the circumstances) provided to Licensee by InvenSense
-hereunder (collectively, "Confidential Information") constitute the
-confidential and proprietary information of InvenSense, and that Licensee's
-protection thereof is an essential condition to Licensee's use and possession
-of the Software. Licensee shall retain all Confidential Information in
-strict confidence and not disclose it to any third party or use it in any way
-except under a written agreement with terms and conditions at least as
-protective as the terms of this Section. Licensee will exercise at least the
-same amount of diligence in preserving the secrecy of the Confidential
-Information as it uses in preserving the secrecy of its own most valuable
-confidential information, but in no event less than reasonable diligence.
-Information shall not be considered Confidential Information if and to the
-extent that it: (i) was in the public domain at the time it was disclosed or
-has entered the public domain through no fault of Licensee; (ii) was known to
-Licensee, without restriction, at the time of disclosure as proven by the
-files of Licensee in existence at the time of disclosure; or (iii) becomes
-known to Licensee, without restriction, from a source other than InvenSense
-without breach of this Agreement by Licensee and otherwise not in violation
-of InvenSense's rights.
-
-5.2. Return of Confidential Information. Notwithstanding the foregoing, all
-documents and other tangible objects containing or representing InvenSense
-Confidential Information and all copies thereof which are in the possession
-of Licensee shall be and remain the property of InvenSense, and shall be
-promptly returned to InvenSense upon written request by InvenSense or upon
-termination of this Agreement.
-
-6. LIMITATION OF LIABILITY
-TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF
-INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL,
-SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF
-LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR
-OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS
-OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
-DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT
-(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR
-SOFTWARE UNDER THIS AGREEMENT. THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING
-ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY.
-
-7. MISCELLANEOUS
-
-7.1. Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS
-SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND
-REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE
-OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS.
-WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE
-TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED.
-
-7.2 Assignment. This Agreement shall be binding upon and inure to the
-benefit of the parties and their respective successors and assigns, provided,
-however that Licensee may not assign this Agreement or any rights or
-obligation hereunder, directly or indirectly, by operation of law or
-otherwise, without the prior written consent of InvenSense, and any such
-attempted assignment shall be void. Notwithstanding the foregoing, Licensee
-may assign this Agreement to a successor to all or substantially all of its
-business or assets to which this Agreement relates that is not a competitor
-of InvenSense.
-
-7.3. Governing Law; Venue. This Agreement shall be governed by the laws of
-California without regard to any conflict-of-laws rules, and the United
-Nations Convention on Contracts for the International Sale of Goods is hereby
-excluded. The sole jurisdiction and venue for actions related to the subject
-matter hereof shall be the state and federal courts located in the County of
-Orange, California, and both parties hereby consent to such jurisdiction and
-venue.
-
-7.4. Severability. All terms and provisions of this Agreement shall, if
-possible, be construed in a manner which makes them valid, but in the event
-any term or provision of this Agreement is found by a court of competent
-jurisdiction to be illegal or unenforceable, the validity or enforceability
-of the remainder of this Agreement shall not be affected if the illegal or
-unenforceable provision does not materially affect the intent of this
-Agreement. If the illegal or unenforceable provision materially affects the
-intent of the parties to this Agreement, this Agreement shall become
-terminated.
-
-7.5. Equitable Relief. Licensee hereby acknowledges that its breach of this
-Agreement would cause irreparable harm and significant injury to InvenSense
-that may be difficult to ascertain and that a remedy at law would be
-inadequate. Accordingly, Licensee agrees that InvenSense shall have the right
-to seek and obtain immediate injunctive relief to enforce obligations under
-the Agreement in addition to any other rights and remedies it may have.
-
-7.6. Waiver. The waiver of, or failure to enforce, any breach or default
-hereunder shall not constitute the waiver of any other or subsequent breach
-or default.
-
-7.7. Entire Agreement. This Agreement sets forth the entire Agreement
-between the parties and supersedes any and all prior proposals, agreements
-and representations between them, whether written or oral concerning the
-Software. This Agreement may be changed only by mutual agreement of the
-parties in writing.
-
diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp
deleted file mode 100644
index 031ae6e..0000000
--- a/60xx/libsensors_iio/MPLSensor.cpp
+++ /dev/null
@@ -1,2764 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define LOG_NDEBUG 0
-//see also the EXTRA_VERBOSE define in the MPLSensor.h header file
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <float.h>
-#include <poll.h>
-#include <unistd.h>
-#include <dirent.h>
-#include <stdlib.h>
-#include <sys/select.h>
-#include <sys/syscall.h>
-#include <dlfcn.h>
-#include <pthread.h>
-#include <cutils/log.h>
-#include <utils/KeyedVector.h>
-#include <utils/String8.h>
-#include <string.h>
-#include <linux/input.h>
-#include <utils/Atomic.h>
-
-#include "MPLSensor.h"
-#include "MPLSupport.h"
-#include "sensor_params.h"
-#include "local_log_def.h"
-
-#include "invensense.h"
-#include "invensense_adv.h"
-#include "ml_stored_data.h"
-#include "ml_load_dmp.h"
-#include "ml_sysfs_helper.h"
-
-// #define TESTING
-
-#ifdef THIRD_PARTY_ACCEL
-# warning "Third party accel"
-# define USE_THIRD_PARTY_ACCEL (1)
-#else
-# define USE_THIRD_PARTY_ACCEL (0)
-#endif
-
-#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
-
-/******************************************************************************/
-/* MPL interface misc. */
-/******************************************************************************/
-static int hertz_request = 200;
-
-#define DEFAULT_MPL_GYRO_RATE (20000L) //us
-#define DEFAULT_MPL_COMPASS_RATE (20000L) //us
-
-#define DEFAULT_HW_GYRO_RATE (100) //Hz
-#define DEFAULT_HW_ACCEL_RATE (20) //ms
-#define DEFAULT_HW_COMPASS_RATE (20000000L) //ns
-#define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns
-
-/* convert ns to hardware units */
-#define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz
-#define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms
-#define HW_COMPASS_RATE_NS (rate_request) // to ns
-
-/* convert Hz to hardware units */
-#define HW_GYRO_RATE_HZ (hertz_request)
-#define HW_ACCEL_RATE_HZ (1000 / hertz_request)
-#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request)
-
-#define RATE_200HZ 5000000LL
-#define RATE_15HZ 66667000LL
-#define RATE_5HZ 200000000LL
-
-static struct sensor_t sSensorList[] =
-{
- {"MPL Gyroscope", "Invensense", 1,
- SENSORS_GYROSCOPE_HANDLE,
- SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Raw Gyroscope", "Invensense", 1,
- SENSORS_RAW_GYROSCOPE_HANDLE,
- SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Accelerometer", "Invensense", 1,
- SENSORS_ACCELERATION_HANDLE,
- SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Magnetic Field", "Invensense", 1,
- SENSORS_MAGNETIC_FIELD_HANDLE,
- SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Orientation", "Invensense", 1,
- SENSORS_ORIENTATION_HANDLE,
- SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Rotation Vector", "Invensense", 1,
- SENSORS_ROTATION_VECTOR_HANDLE,
- SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Linear Acceleration", "Invensense", 1,
- SENSORS_LINEAR_ACCEL_HANDLE,
- SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
- {"MPL Gravity", "Invensense", 1,
- SENSORS_GRAVITY_HANDLE,
- SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
-
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- {"MPL Screen Orientation", "Invensense ", 1,
- SENSORS_SCREEN_ORIENTATION_HANDLE,
- SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}},
-#endif
-};
-
-MPLSensor *MPLSensor::gMPLSensor = NULL;
-
-extern "C" {
-void procData_cb_wrapper()
-{
- if(MPLSensor::gMPLSensor) {
- MPLSensor::gMPLSensor->cbProcData();
- }
-}
-
-void setCallbackObject(MPLSensor* gbpt)
-{
- MPLSensor::gMPLSensor = gbpt;
-}
-
-MPLSensor* getCallbackObject() {
- return MPLSensor::gMPLSensor;
-}
-
-} // end of extern C
-
-#ifdef INV_PLAYBACK_DBG
-static FILE *logfile = NULL;
-#endif
-
-pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
-
-/*******************************************************************************
- * MPLSensor class implementation
- ******************************************************************************/
-
-MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
- : SensorBase(NULL, NULL),
- mNewData(0),
- mMasterSensorMask(INV_ALL_SENSORS),
- mLocalSensorMask(0),
- mPollTime(-1),
- mHaveGoodMpuCal(0),
- mGyroAccuracy(0),
- mAccelAccuracy(0),
- mCompassAccuracy(0),
- mSampleCount(0),
- dmp_orient_fd(-1),
- mDmpOrientationEnabled(0),
- mEnabled(0),
- mOldEnabledMask(0),
- mAccelInputReader(4),
- mGyroInputReader(32),
- mTempScale(0),
- mTempOffset(0),
- mTempCurrentTime(0),
- mAccelScale(2),
- mPendingMask(0),
- mSensorMask(0),
- mFeatureActiveMask(0) {
- VFUNC_LOG;
-
- inv_error_t rv;
- int fd;
- char *ver_str;
-
- mCompassSensor = compass;
-
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:MPLSensor constructor : numSensors = %d", numSensors);
-
- pthread_mutex_init(&mMplMutex, NULL);
- pthread_mutex_init(&mHALMutex, NULL);
- memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
- memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
-
-#ifdef INV_PLAYBACK_DBG
- LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
- logfile = fopen("/data/playback.bin", "wb");
- if (logfile)
- inv_turn_on_data_logging(logfile);
-#endif
-
- /* setup sysfs paths */
- inv_init_sysfs_attributes();
-
- /* get chip name */
- if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
- LOGE("HAL:ERR- Failed to get chip ID\n");
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
- }
-
- enable_iio_sysfs();
-
- /* turn on power state */
- onPower(1);
-
- /* reset driver master enable */
- masterEnable(0);
-
- if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
- /* Load DMP image if capable, ie. MPU6xxx/9xxx */
- loadDMP();
- }
-
- /* open temperature fd for temp comp */
- LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
- gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
- if (gyro_temperature_fd == -1) {
- LOGE("HAL:could not open temperature node");
- } else {
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:temperature_fd opened: %s", mpu.temperature);
- }
-
- /* read accel FSR to calcuate accel scale later */
- if (!USE_THIRD_PARTY_ACCEL) {
- char buf[3];
- int count = 0;
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
-
- fd = open(mpu.accel_fsr, O_RDONLY);
- if(fd < 0) {
- LOGE("HAL:Error opening accel FSR");
- } else {
- memset(buf, 0, sizeof(buf));
- count = read_attribute_sensor(fd, buf, sizeof(buf));
- if(count < 1) {
- LOGE("HAL:Error reading accel FSR");
- } else {
- count = sscanf(buf, "%d", &mAccelScale);
- if(count)
- LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
- }
- close(fd);
- }
- }
-
- /* initialize sensor data */
- memset(mPendingEvents, 0, sizeof(mPendingEvents));
-
- mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
- mPendingEvents[RotationVector].sensor = ID_RV;
- mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
-
- mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
- mPendingEvents[LinearAccel].sensor = ID_LA;
- mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
-
- mPendingEvents[Gravity].version = sizeof(sensors_event_t);
- mPendingEvents[Gravity].sensor = ID_GR;
- mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
-
- mPendingEvents[Gyro].version = sizeof(sensors_event_t);
- mPendingEvents[Gyro].sensor = ID_GY;
- mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
-
- mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
- mPendingEvents[RawGyro].sensor = ID_RG;
- mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
-
- mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
- mPendingEvents[Accelerometer].sensor = ID_A;
- mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
-
- /* Invensense compass calibration */
- mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
- mPendingEvents[MagneticField].sensor = ID_M;
- mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
- mPendingEvents[MagneticField].magnetic.status =
- SENSOR_STATUS_ACCURACY_HIGH;
-
- mPendingEvents[Orientation].version = sizeof(sensors_event_t);
- mPendingEvents[Orientation].sensor = ID_O;
- mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
- mPendingEvents[Orientation].orientation.status
- = SENSOR_STATUS_ACCURACY_HIGH;
-
- mHandlers[RotationVector] = &MPLSensor::rvHandler;
- mHandlers[LinearAccel] = &MPLSensor::laHandler;
- mHandlers[Gravity] = &MPLSensor::gravHandler;
- mHandlers[Gyro] = &MPLSensor::gyroHandler;
- mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
- mHandlers[Accelerometer] = &MPLSensor::accelHandler;
- mHandlers[MagneticField] = &MPLSensor::compassHandler;
- mHandlers[Orientation] = &MPLSensor::orienHandler;
-
- for (int i = 0; i < numSensors; i++) {
- mDelays[i] = 0;
- }
-
- (void)inv_get_version(&ver_str);
- LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
-
- /* setup MPL */
- inv_constructor_init();
-
- /* load calibration file from /data/inv_cal_data.bin */
- rv = inv_load_calibration();
- if(rv == INV_SUCCESS)
- LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
- else
- LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
-
- /* Takes external Accel Calibration Load Method */
- if( m_pt2AccelCalLoadFunc != NULL)
- {
- long accel_offset[3];
- long tmp_offset[3];
- int result = m_pt2AccelCalLoadFunc(accel_offset);
- if(result)
- LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
- else
- {
- LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
- inv_get_accel_bias(tmp_offset, NULL);
- LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
- tmp_offset[0], tmp_offset[1], tmp_offset[2]);
- inv_set_accel_bias(accel_offset, mAccelAccuracy);
- inv_get_accel_bias(tmp_offset, NULL);
- LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
- tmp_offset[0], tmp_offset[1], tmp_offset[2]);
- }
- }
- /* End of Accel Calibration Load Method */
-
- inv_set_device_properties();
-
- /* disable driver master enable the first sensor goes on */
- masterEnable(0);
- enableGyro(0);
- enableAccel(0);
- enableCompass(0);
-
- if (isLowPowerQuatEnabled()) {
- enableLPQuaternion(0);
- }
-
- onPower(0);
-
- if (isDmpDisplayOrientationOn()) {
- enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
- }
-
-}
-
-void MPLSensor::enable_iio_sysfs()
-{
- VFUNC_LOG;
-
- char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
- FILE *tempFp = NULL;
-
- /* ignore failures */
- write_sysfs_int(mpu.in_timestamp_en, 1);
-
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)",
- mpu.trigger_name, getTimestamp());
- tempFp = fopen(mpu.trigger_name, "r");
- if (tempFp == NULL) {
- LOGE("HAL:could not open trigger name");
- } else {
- if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
- LOGE("HAL:could not read trigger name");
- }
- fclose(tempFp);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
- iio_trigger_name, mpu.current_trigger, getTimestamp());
- tempFp = fopen(mpu.current_trigger, "w");
- if (tempFp == NULL) {
- LOGE("HAL:could not open current trigger");
- } else {
- if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
- LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
- }
- }
-
- write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
-
- if (inv_get_iio_device_node(iio_device_node) < 0) {
- LOGE("HAL:could retrive the iio device node");
- }
- iio_fd = open(iio_device_node, O_RDONLY);
- if (iio_fd < 0) {
- LOGE("HAL:could not open iio device node");
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
- }
-}
-
-int MPLSensor::inv_constructor_init()
-{
- VFUNC_LOG;
-
- inv_error_t result = inv_init_mpl();
- if (result) {
- LOGE("HAL:inv_init_mpl() failed");
- return result;
- }
- result = inv_constructor_default_enable();
- result = inv_start_mpl();
- if (result) {
- LOGE("HAL:inv_start_mpl() failed");
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-int MPLSensor::inv_constructor_default_enable()
-{
- VFUNC_LOG;
-
- inv_error_t result;
-
- result = inv_enable_quaternion();
- if (result) {
- LOGE("HAL:Cannot enable quaternion\n");
- return result;
- }
-
- result = inv_enable_in_use_auto_calibration();
- if (result) {
- return result;
- }
-
- // result = inv_enable_motion_no_motion();
- result = inv_enable_fast_nomot();
- if (result) {
- return result;
- }
-
- result = inv_enable_gyro_tc();
- if (result) {
- return result;
- }
-
- result = inv_enable_hal_outputs();
- if (result) {
- return result;
- }
-
- if (!mCompassSensor->providesCalibration()) {
- /* Invensense compass calibration */
- LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
- result = inv_enable_vector_compass_cal();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- } else {
- mFeatureActiveMask |= INV_COMPASS_CAL;
- }
-
- // specify MPL's trust weight, used by compass algorithms
- inv_vector_compass_cal_sensitivity(3);
-
- result = inv_enable_compass_bias_w_gyro();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_enable_heading_from_gyro();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_enable_magnetic_disturbance();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- result = inv_enable_9x_sensor_fusion();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- } else {
- // 9x sensor fusion enables Compass fit
- mFeatureActiveMask |= INV_COMPASS_FIT;
- }
-
- result = inv_enable_no_gyro_fusion();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_enable_quat_accuracy_monitor();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-/* TODO: create function pointers to calculate scale */
-void MPLSensor::inv_set_device_properties()
-{
- VFUNC_LOG;
-
- unsigned short orient;
-
- inv_get_sensors_orientation();
-
- inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
- inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
-
- /* gyro setup */
- orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
- inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
-
- /* accel setup */
- orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
- /* use for third party accel input subsystem driver
- inv_set_accel_orientation_and_scale(orient, 1LL << 22);
- */
- inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
-
- /* compass setup */
- signed char orientMtx[9];
- mCompassSensor->getOrientationMatrix(orientMtx);
- orient =
- inv_orientation_matrix_to_scalar(orientMtx);
- long sensitivity;
- sensitivity = mCompassSensor->getSensitivity();
- inv_set_compass_orientation_and_scale(orient, sensitivity);
-}
-
-void MPLSensor::loadDMP()
-{
- int res, fd;
- FILE *fptr;
-
- if (isMpu3050()) {
- //DMP support only for MPU6xxx/9xxx currently
- return;
- }
-
- /* load DMP firmware */
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
- fd = open(mpu.firmware_loaded, O_RDONLY);
- if(fd < 0) {
- LOGE("HAL:could not open dmp state");
- return;
- }
- if(inv_read_dmp_state(fd)) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
- return;
- }
-
- LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
- fptr = fopen(mpu.dmp_firmware, "w");
- if(!fptr) {
- LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
- return;
- }
- res = inv_load_dmp(fptr);
- if(res < 0) {
- LOGE("HAL:load DMP failed");
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
- }
- fclose(fptr);
-}
-
-void MPLSensor::inv_get_sensors_orientation()
-{
- FILE *fptr;
-
- // get gyro orientation
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
- fptr = fopen(mpu.gyro_orient, "r");
- if (fptr != NULL) {
- int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
- &om[6], &om[7], &om[8]);
- fclose(fptr);
-
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:gyro mounting matrix: "
- "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
- om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
-
- mGyroOrientation[0] = om[0];
- mGyroOrientation[1] = om[1];
- mGyroOrientation[2] = om[2];
- mGyroOrientation[3] = om[3];
- mGyroOrientation[4] = om[4];
- mGyroOrientation[5] = om[5];
- mGyroOrientation[6] = om[6];
- mGyroOrientation[7] = om[7];
- mGyroOrientation[8] = om[8];
- } else {
- LOGE("HAL:Couldn't read gyro mounting matrix");
- }
-
- // get accel orientation
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
- fptr = fopen(mpu.accel_orient, "r");
- if (fptr != NULL) {
- int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
- &om[6], &om[7], &om[8]);
- fclose(fptr);
-
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:accel mounting matrix: "
- "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
- om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
-
- mAccelOrientation[0] = om[0];
- mAccelOrientation[1] = om[1];
- mAccelOrientation[2] = om[2];
- mAccelOrientation[3] = om[3];
- mAccelOrientation[4] = om[4];
- mAccelOrientation[5] = om[5];
- mAccelOrientation[6] = om[6];
- mAccelOrientation[7] = om[7];
- mAccelOrientation[8] = om[8];
- } else {
- LOGE("HAL:Couldn't read accel mounting matrix");
- }
-}
-
-MPLSensor::~MPLSensor()
-{
- VFUNC_LOG;
-
- mCompassSensor = NULL;
-
- /* Close open fds */
- if (iio_fd > 0)
- close(iio_fd);
- if( accel_fd > 0 )
- close(accel_fd );
- if (gyro_temperature_fd > 0)
- close(gyro_temperature_fd);
- if (sysfs_names_ptr)
- free(sysfs_names_ptr);
-
- if (isDmpDisplayOrientationOn()) {
- closeDmpOrientFd();
- }
-
- /* Turn off Gyro master enable */
- /* A workaround until driver handles it */
- /* TODO: Turn off and close all sensors */
- if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
- LOGE("HAL:could not disable gyro master enable");
- }
-
-#ifdef INV_PLAYBACK_DBG
- inv_turn_off_data_logging();
- fclose(logfile);
-#endif
-}
-
-#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
-#define A_ENABLED ((1 << ID_A) & enabled_sensors)
-#define M_ENABLED ((1 << ID_M) & enabled_sensors)
-#define O_ENABLED ((1 << ID_O) & enabled_sensors)
-#define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
-#define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
-#define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
-
-/* TODO: this step is optional, remove? */
-int MPLSensor::setGyroInitialState()
-{
- VFUNC_LOG;
-
- int res = 0;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
- int fd = open(mpu.gyro_fifo_rate, O_RDWR);
- res = errno;
- if(fd < 0) {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.gyro_fifo_rate, strerror(res), res);
- return res;
- }
- res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
- if(res < 0) {
- LOGE("HAL:write_attribute_sensor : error writing %s with %d",
- mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
- return res;
- }
-
- // Setting LPF is deprecated
-
- return 0;
-}
-
-/* this applies to BMA250 Input Subsystem Driver only */
-int MPLSensor::setAccelInitialState()
-{
- VFUNC_LOG;
-
- struct input_absinfo absinfo_x;
- struct input_absinfo absinfo_y;
- struct input_absinfo absinfo_z;
- float value;
- if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
- !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
- !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
- value = absinfo_x.value;
- mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
- value = absinfo_y.value;
- mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
- value = absinfo_z.value;
- mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
- }
- return 0;
-}
-
-int MPLSensor::onPower(int en)
-{
- VFUNC_LOG;
-
- int res;
-
- int curr_power_state;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, mpu.power_state, getTimestamp());
- res = read_sysfs_int(mpu.power_state, &curr_power_state);
- if (res < 0) {
- LOGE("HAL:Error reading power state");
- // will set power_state anyway
- curr_power_state = -1;
- }
- if (en != curr_power_state) {
- if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
- LOGE("HAL:Couldn't write power state");
- }
- } else {
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:Power state already enable/disable curr=%d new=%d",
- curr_power_state, en);
- }
- return res;
-}
-
-int MPLSensor::onDMP(int en)
-{
- VFUNC_LOG;
-
- int res = -1;
- int status;
-
- //Sequence to enable DMP
- //1. Turn On power if not already on
- //2. Load DMP image if not already loaded
- //3. Either Gyro or Accel must be enabled/configured before next step
- //4. Enable DMP
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- mpu.firmware_loaded, getTimestamp());
- res = read_sysfs_int(mpu.firmware_loaded, &status);
- if (res < 0){
- LOGE("HAL:ERR can't get firmware_loaded status");
- return res;
- }
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
-
- if (status) {
- //Write only if curr DMP state <> request
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
- mpu.dmp_on, getTimestamp());
- if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
- LOGE("HAL:ERR can't read DMP state");
- } else if (status != en) {
- res = write_sysfs_int(mpu.dmp_on, en);
- //Enable DMP interrupt
- if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
- LOGE("HAL:ERR can't en/dis DMP interrupt");
- }
- } else {
- res = 0; //DMP already set as requested
- }
- } else {
- LOGE("HAL:ERR No DMP image");
- }
- return res;
-}
-
-int MPLSensor::checkLPQuaternion(void)
-{
- VFUNC_LOG;
-
- return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
-}
-
-int MPLSensor::enableLPQuaternion(int en)
-{
- VFUNC_LOG;
-
- if (!en) {
- enableQuaternionData(0);
- onDMP(0);
- mFeatureActiveMask &= ~INV_DMP_QUATERNION;
- LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
- } else {
- if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
- LOGE("HAL:ERR can't enable LP Quaternion");
- } else {
- mFeatureActiveMask |= INV_DMP_QUATERNION;
- LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
- }
- }
- return 0;
-}
-
-int MPLSensor::enableQuaternionData(int en)
-{
- int res = 0;
- VFUNC_LOG;
-
- // Enable DMP quaternion
- res = write_sysfs_int(mpu.quaternion_on, en);
-
- if (!en) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
- } else {
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
- }
- write_sysfs_int(mpu.in_quat_r_en, en);
- write_sysfs_int(mpu.in_quat_x_en, en);
- write_sysfs_int(mpu.in_quat_y_en, en);
- write_sysfs_int(mpu.in_quat_z_en, en);
-
- LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
-
- if (!en) {
- inv_quaternion_sensor_was_turned_off();
- }
-
- return res;
-}
-
-int MPLSensor::enableTap(int /*en*/)
-{
- VFUNC_LOG;
-
- return 0;
-}
-
-int MPLSensor::enableFlick(int /*en*/)
-{
- VFUNC_LOG;
-
- return 0;
-}
-
-int MPLSensor::enablePedometer(int /*en*/)
-{
- VFUNC_LOG;
-
- return 0;
-}
-
-int MPLSensor::masterEnable(int en)
-{
- VFUNC_LOG;
- return write_sysfs_int(mpu.chip_enable, en);
-}
-
-int MPLSensor::enableGyro(int en)
-{
- VFUNC_LOG;
-
- /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
- int res;
-
- /* need to also turn on/off the master enable */
- res = write_sysfs_int(mpu.gyro_enable, en);
-
- if (!en) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
- inv_gyro_was_turned_off();
- } else {
- write_sysfs_int(mpu.gyro_x_fifo_enable, en);
- write_sysfs_int(mpu.gyro_y_fifo_enable, en);
- res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
- }
-
- return res;
-}
-
-int MPLSensor::enableAccel(int en)
-{
- VFUNC_LOG;
-
- /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
- int res;
-
- /* need to also turn on/off the master enable */
- res = write_sysfs_int(mpu.accel_enable, en);
-
- if (!en) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
- inv_accel_was_turned_off();
- } else {
- write_sysfs_int(mpu.accel_x_fifo_enable, en);
- write_sysfs_int(mpu.accel_y_fifo_enable, en);
- res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
- }
-
- return res;
-}
-
-int MPLSensor::enableCompass(int en)
-{
- VFUNC_LOG;
-
- int res = mCompassSensor->enable(ID_M, en);
- if (!en) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
- inv_compass_was_turned_off();
- }
- return res;
-}
-
-void MPLSensor::computeLocalSensorMask(int enabled_sensors)
-{
- VFUNC_LOG;
-
- do {
- if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
- LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
- mLocalSensorMask = ALL_MPL_SENSORS_NP;
- break;
- }
-
- if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
- /* Invensense compass cal */
- LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
- mLocalSensorMask = 0;
- break;
- }
-
- if (GY_ENABLED) {
- LOGV_IF(ENG_VERBOSE, "G ENABLED");
- mLocalSensorMask |= INV_THREE_AXIS_GYRO;
- } else {
- LOGV_IF(ENG_VERBOSE, "G DISABLED");
- mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
- }
-
- if (A_ENABLED) {
- LOGV_IF(ENG_VERBOSE, "A ENABLED");
- mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
- } else {
- LOGV_IF(ENG_VERBOSE, "A DISABLED");
- mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
- }
-
- /* Invensense compass calibration */
- if (M_ENABLED) {
- LOGV_IF(ENG_VERBOSE, "M ENABLED");
- mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
- } else {
- LOGV_IF(ENG_VERBOSE, "M DISABLED");
- mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
- }
- } while (0);
-}
-
-int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
- return (this->*enabler)(en);
-}
-
-int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) {
- VFUNC_LOG;
-
- inv_error_t res = -1;
- bool store_cal = false;
- bool ext_compass_changed = false;
-
- // Sequence to enable or disable a sensor
- // 1. enable Power state
- // 2. reset master enable (=0)
- // 3. enable or disable a sensor
- // 4. set master enable (=1)
-
- pthread_mutex_lock(&GlobalHalMutex);
-
- uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
- | (1 << MagneticField);
- uint32_t all_integrated_changeables = all_changeables;
-
- if (!mCompassSensor->isIntegrated()) {
- ext_compass_changed = changed & (1 << MagneticField);
- all_integrated_changeables = all_changeables & ~(1 << MagneticField);
- }
-
- if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
- /* ensure power state is on */
- onPower(1);
-
- /* reset master enable */
- res = masterEnable(0);
- if(res < 0) {
- goto unlock_res;
- }
- }
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
-
- if (changed & ((1 << Gyro) | (1 << RawGyro))) {
- res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
- if(res < 0) {
- goto unlock_res;
- }
- }
-
- if (changed & (1 << Accelerometer)) {
- res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
- if(res < 0) {
- goto unlock_res;
- }
- }
-
- if (changed & (1 << MagneticField)) {
- /* Invensense compass calibration */
- res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
- if(res < 0) {
- goto unlock_res;
- }
- }
-
- if ( isLowPowerQuatEnabled() ) {
- // Enable LP Quat
- if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
- (1 << LinearAccel) | (1 << Gravity)))) {
- if (!(changed & all_integrated_changeables)) {
- /* ensure power state is on */
- onPower(1);
- /* reset master enable */
- res = masterEnable(0);
- if(res < 0) {
- goto unlock_res;
- }
- }
- if (!checkLPQuaternion()) {
- enableLPQuaternion(1);
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
- }
- } else if (checkLPQuaternion()) {
- enableLPQuaternion(0);
- }
- }
-
- if (changed & all_integrated_changeables) {
- if (sensors &
- (INV_THREE_AXIS_GYRO
- | INV_THREE_AXIS_ACCEL
- | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
- if ( isLowPowerQuatEnabled() ||
- (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
- // disable DMP event interrupt only (w/ data interrupt)
- if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
- res = -1;
- LOGE("HAL:ERR can't disable DMP event interrupt");
- goto unlock_res;
- }
- }
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- // enable DMP
- onDMP(1);
-
- res = enableAccel(1);
- if(res < 0) {
- goto unlock_res;
- }
-
- if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
- res = turnOffAccelFifo();
- }
- if(res < 0) {
- goto unlock_res;
- }
- }
-
- res = masterEnable(1);
- if(res < 0) {
- goto unlock_res;
- }
- } else { // all sensors idle -> reduce power
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- // enable DMP
- onDMP(1);
- // enable DMP event interrupt only (no data interrupt)
- if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
- res = -1;
- LOGE("HAL:ERR can't enable DMP event interrupt");
- }
- res = enableAccel(1);
- if(res < 0) {
- goto unlock_res;
- }
- if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
- res = turnOffAccelFifo();
- }
- if(res < 0) {
- goto unlock_res;
- }
- res = masterEnable(1);
- if(res < 0) {
- goto unlock_res;
- }
- }
- else {
- res = onPower(0);
- if(res < 0) {
- goto unlock_res;
- }
- }
- store_cal = true;
- }
- } else if (ext_compass_changed &&
- !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
- | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
- store_cal = true;
- }
-
- if (store_cal || ((changed & all_changeables) != all_changeables)) {
- storeCalibration();
- }
-
-unlock_res:
- pthread_mutex_unlock(&GlobalHalMutex);
- return res;
-}
-
-/* Store calibration file */
-void MPLSensor::storeCalibration()
-{
- if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
- int res = inv_store_calibration();
- if (res) {
- LOGE("HAL:Cannot store calibration on file");
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
- }
- }
-}
-
-void MPLSensor::cbProcData()
-{
- mNewData = 1;
- mSampleCount++;
- LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
-}
-
-/* these handlers transform mpl data into one of the Android sensor types */
-int MPLSensor::gyroHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int8_t status;
- int update;
- update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
- s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
- return update;
-}
-
-int MPLSensor::rawGyroHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int8_t status;
- int update;
- update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
- s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
- return update;
-}
-
-int MPLSensor::accelHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int8_t status;
- int update;
- update = inv_get_sensor_type_accelerometer(
- s->acceleration.v, &status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
- s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
- s->timestamp, update);
- mAccelAccuracy = status;
- return update;
-}
-
-int MPLSensor::compassHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int update;
- update = inv_get_sensor_type_magnetic_field(
- s->magnetic.v, &s->magnetic.status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
- s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
- mCompassAccuracy = s->magnetic.status;
- return update;
-}
-
-int MPLSensor::rvHandler(sensors_event_t* s)
-{
- // rotation vector does not have an accuracy or status
- VHANDLER_LOG;
- int8_t status;
- int update;
- update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
- s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
- return update;
-}
-
-int MPLSensor::laHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int8_t status;
- int update;
- update = inv_get_sensor_type_linear_acceleration(
- s->gyro.v, &status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
- s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
- return update;
-}
-
-int MPLSensor::gravHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int8_t status;
- int update;
- update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
- s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
- return update;
-}
-
-int MPLSensor::orienHandler(sensors_event_t* s)
-{
- VHANDLER_LOG;
- int update;
- update = inv_get_sensor_type_orientation(
- s->orientation.v, &s->orientation.status, &s->timestamp);
- LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
- s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
- return update;
-}
-
-int MPLSensor::enable(int32_t handle, int en)
-{
- VFUNC_LOG;
-
- android::String8 sname;
- int what = -1, err = 0;
-
- switch (handle) {
- case ID_SO:
- sname = "Screen Orientation";
- LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
- (mDmpOrientationEnabled? "en": "dis"),
- (en? "en" : "dis"));
- enableDmpOrientation(en && isDmpDisplayOrientationOn());
- /* TODO: stop manually testing/using 0 and 1 instead of
- * false and true, but just use 0 and non-0.
- * This allows passing 0 and non-0 ints around instead of
- * having to convert to 1 and test against 1.
- */
- mDmpOrientationEnabled = en;
- return 0;
- case ID_A:
- what = Accelerometer;
- sname = "Accelerometer";
- break;
- case ID_M:
- what = MagneticField;
- sname = "MagneticField";
- break;
- case ID_O:
- what = Orientation;
- sname = "Orientation";
- break;
- case ID_GY:
- what = Gyro;
- sname = "Gyro";
- break;
- case ID_RG:
- what = RawGyro;
- sname = "RawGyro";
- break;
- case ID_GR:
- what = Gravity;
- sname = "Gravity";
- break;
- case ID_RV:
- what = RotationVector;
- sname = "RotationVector";
- break;
- case ID_LA:
- what = LinearAccel;
- sname = "LinearAccel";
- break;
- default: //this takes care of all the gestures
- what = handle;
- sname = "Others";
- break;
- }
-
- if (uint32_t(what) >= numSensors)
- return -EINVAL;
-
- int newState = en ? 1 : 0;
- unsigned long sen_mask;
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
- ((mEnabled & (1 << what)) ? "en" : "dis"),
- ((uint32_t(newState) << what) ? "en" : "dis"));
- LOGV_IF(PROCESS_VERBOSE,
- "HAL:%s sensor state change what=%d", sname.string(), what);
-
- if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
- short flags = newState;
- uint32_t lastEnabled = mEnabled, changed = 0;
-
- mEnabled &= ~(1 << what);
- mEnabled |= (uint32_t(flags) << what);
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
- LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
- computeLocalSensorMask(mEnabled);
- LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
- sen_mask = mLocalSensorMask & mMasterSensorMask;
- mSensorMask = sen_mask;
- LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
-
- switch (what) {
- case Gyro:
- case RawGyro:
- case Accelerometer:
- case MagneticField:
- if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
- (1 << LinearAccel) | (1 << Gravity))) &&
- ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
- changed |= (1 << what);
- }
- break;
-
- case Orientation:
- case RotationVector:
- case LinearAccel:
- case Gravity:
- if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
- (1 << LinearAccel) | (1 << Gravity)))) ||
- (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
- (1 << LinearAccel) | (1 << Gravity))))) {
- for (int i = Gyro; i <= MagneticField; i++) {
- if (!(mEnabled & (1 << i))) {
- changed |= (1 << i);
- }
- }
- }
- break;
- }
- LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
- enableSensors(sen_mask, flags, changed);
- }
-
-#ifdef INV_PLAYBACK_DBG
- /* apparently the logging needs to be go through this sequence
- to properly flush the log file */
- inv_turn_off_data_logging();
- fclose(logfile);
- logfile = fopen("/data/playback.bin", "ab");
- if (logfile)
- inv_turn_on_data_logging(logfile);
-#endif
-
- return err;
-}
-
-int MPLSensor::setDelay(int32_t handle, int64_t ns)
-{
- VFUNC_LOG;
-
- android::String8 sname;
- int what = -1;
-
- switch (handle) {
- case ID_SO:
- return update_delay();
- case ID_A:
- what = Accelerometer;
- sname = "Accelerometer";
- break;
- case ID_M:
- what = MagneticField;
- sname = "MagneticField";
- break;
- case ID_O:
- what = Orientation;
- sname = "Orientation";
- break;
- case ID_GY:
- what = Gyro;
- sname = "Gyro";
- break;
- case ID_RG:
- what = RawGyro;
- sname = "RawGyro";
- break;
- case ID_GR:
- what = Gravity;
- sname = "Gravity";
- break;
- case ID_RV:
- what = RotationVector;
- sname = "RotationVector";
- break;
- case ID_LA:
- what = LinearAccel;
- sname = "LinearAccel";
- break;
- default: // this takes care of all the gestures
- what = handle;
- sname = "Others";
- break;
- }
-
-#if 0
- // skip the 1st call for enalbing sensors called by ICS/JB sensor service
- static int counter_delay = 0;
- if (!(mEnabled & (1 << what))) {
- counter_delay = 0;
- } else {
- if (++counter_delay == 1) {
- return 0;
- }
- else {
- counter_delay = 0;
- }
- }
-#endif
-
- if (uint32_t(what) >= numSensors)
- return -EINVAL;
-
- if (ns < 0)
- return -EINVAL;
-
- LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
-
- // limit all rates to reasonable ones */
- if (ns < 5000000LL) {
- ns = 5000000LL;
- }
-
- /* store request rate to mDelays array for each sensor */
- mDelays[what] = ns;
-
- switch (what) {
- case Gyro:
- case RawGyro:
- case Accelerometer:
- for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
- i++) {
- if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
- return 0;
- }
- }
- break;
-
- case MagneticField:
- if (mCompassSensor->isIntegrated() &&
- (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
- ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
- ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
- return 0;
- }
- break;
-
- case Orientation:
- case RotationVector:
- case LinearAccel:
- case Gravity:
- if (isLowPowerQuatEnabled()) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
- break;
- }
-
- for (int i = 0; i < numSensors; i++) {
- if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
- return 0;
- }
- }
- break;
- }
-
- int res = update_delay();
- return res;
-}
-
-int MPLSensor::update_delay() {
- VHANDLER_LOG;
-
- int res = 0;
- int64_t got;
-
- pthread_mutex_lock(&GlobalHalMutex);
- if (mEnabled) {
- int64_t wanted = 1000000000;
- int64_t wanted_3rd_party_sensor = 1000000000;
-
- // Sequence to change sensor's FIFO rate
- // 1. enable Power state
- // 2. reset master enable
- // 3. Update delay
- // 4. set master enable
-
- // ensure power on
- onPower(1);
-
- // reset master enable
- masterEnable(0);
-
- /* search the minimum delay requested across all enabled sensors */
- for (int i = 0; i < numSensors; i++) {
- if (mEnabled & (1 << i)) {
- int64_t ns = mDelays[i];
- wanted = wanted < ns ? wanted : ns;
- }
- }
-
- // same delay for 3rd party Accel or Compass
- wanted_3rd_party_sensor = wanted;
-
- /* mpl rate in us in future maybe different for
- gyro vs compass vs accel */
- int rateInus = (int)wanted / 1000LL;
- int mplGyroRate = rateInus;
- int mplAccelRate = rateInus;
- int mplCompassRate = rateInus;
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
- "%llu ns, mpl rate: %d us, (%.2f Hz)",
- wanted, rateInus, 1000000000.f / wanted);
-
- /* set rate in MPL */
- /* compass can only do 100Hz max */
- inv_set_gyro_sample_rate(mplGyroRate);
- inv_set_accel_sample_rate(mplAccelRate);
- inv_set_compass_sample_rate(mplCompassRate);
-#ifdef LIBMLLITE_FROM_SOURCE
- inv_set_linear_acceleration_sample_rate(rateInus);
- inv_set_gravity_sample_rate(rateInus);
-#endif
-
- /* TODO: Test 200Hz */
- // inv_set_gyro_sample_rate(5000);
- LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
- LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
- LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
-
- int enabled_sensors = mEnabled;
- int tempFd = -1;
- if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
- if (isLowPowerQuatEnabled() ||
- (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
- bool setDMPrate= 0;
- // Set LP Quaternion sample rate if enabled
- if (checkLPQuaternion()) {
- if (wanted < RATE_200HZ) {
- enableLPQuaternion(0);
- } else {
- inv_set_quat_sample_rate(rateInus);
- setDMPrate= 1;
- }
- }
-
- if (checkDMPOrientation() || setDMPrate==1) {
- getDmpRate(&wanted);
- }
- }
-
- int64_t tempRate = wanted;
- LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
- //nsToHz
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / tempRate, mpu.gyro_fifo_rate,
- getTimestamp());
- tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
- if(res < 0) {
- LOGE("HAL:GYRO update delay error");
- }
-
- //nsToHz (BMA250)
- if(USE_THIRD_PARTY_ACCEL) {
- LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
- wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
- getTimestamp());
- tempFd = open(mpu.accel_fifo_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
- LOGE_IF(res < 0, "HAL:ACCEL update delay error");
- }
-
- if (!mCompassSensor->isIntegrated()) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
- mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
- got = mCompassSensor->getDelay(ID_M);
- inv_set_compass_sample_rate(got / 1000);
- }
-
- } else {
-
- if (GY_ENABLED) {
- wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
- (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
- (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- getDmpRate(&wanted);
- }
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
- tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
- LOGE_IF(res < 0, "HAL:GYRO update delay error");
- }
-
- if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
- if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
- wanted = mDelays[Gyro];
- }
- else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
- wanted = mDelays[RawGyro];
-
- } else {
- wanted = mDelays[Accelerometer];
- }
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- getDmpRate(&wanted);
- }
-
- /* TODO: use function pointers to calculate delay value specific to vendor */
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
- tempFd = open(mpu.accel_fifo_rate, O_RDWR);
- if(USE_THIRD_PARTY_ACCEL) {
- //BMA250 in ms
- res = write_attribute_sensor(tempFd, wanted / 1000000L);
- }
- else {
- //MPUxxxx in hz
- res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
- }
- LOGE_IF(res < 0, "HAL:ACCEL update delay error");
- }
-
- /* Invensense compass calibration */
- if (M_ENABLED) {
- if (!mCompassSensor->isIntegrated()) {
- wanted = mDelays[MagneticField];
- } else {
- if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
- wanted = mDelays[Gyro];
- }
- else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
- wanted = mDelays[RawGyro];
- } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
- wanted = mDelays[Accelerometer];
- } else {
- wanted = mDelays[MagneticField];
- }
-
- if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
- getDmpRate(&wanted);
- }
- }
-
- mCompassSensor->setDelay(ID_M, wanted);
- got = mCompassSensor->getDelay(ID_M);
- inv_set_compass_sample_rate(got / 1000);
- }
-
- }
-
- unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
- if (sensors &
- (INV_THREE_AXIS_GYRO
- | INV_THREE_AXIS_ACCEL
- | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
- res = masterEnable(1);
- } else { // all sensors idle -> reduce power
- res = onPower(0);
- }
- }
- pthread_mutex_unlock(&GlobalHalMutex);
- return res;
-}
-
-/* For Third Party Accel Input Subsystem Drivers only */
-/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
-int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count)
-{
- VHANDLER_LOG;
-
- if (count < 1)
- return -EINVAL;
-
- ssize_t n = mAccelInputReader.fill(accel_fd);
- if (n < 0) {
- LOGE("HAL:missed accel events, exit");
- return n;
- }
-
- int numEventReceived = 0;
- input_event const* event;
- int done = 0;
-
- while (!done && count && mAccelInputReader.readEvent(&event)) {
- int type = event->type;
- if (type == EV_ABS) {
- if (event->code == EVENT_TYPE_ACCEL_X) {
- mPendingMask |= 1 << Accelerometer;
- mCachedAccelData[0] = event->value;
- } else if (event->code == EVENT_TYPE_ACCEL_Y) {
- mPendingMask |= 1 << Accelerometer;
- mCachedAccelData[1] = event->value;
- } else if (event->code == EVENT_TYPE_ACCEL_Z) {
- mPendingMask |= 1 << Accelerometer;
- mCachedAccelData[2] =event-> value;
- }
- } else if (type == EV_SYN) {
- done = 1;
- if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
- inv_build_accel(mCachedAccelData, 0, getTimestamp());
- }
- } else {
- LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
- type, event->code);
- }
- mAccelInputReader.next();
- }
-
- return numEventReceived;
-}
-
-/**
- * Should be called after reading at least one of gyro
- * compass or accel data. (Also okay for handling all of them).
- * @returns 0, if successful, error number if not.
- */
-/* TODO: This should probably be called "int readEvents(...)"
- * and readEvents() called "void cacheData(void)".
- */
-int MPLSensor::executeOnData(sensors_event_t* data, int count)
-{
- VFUNC_LOG;
-
- inv_execute_on_data();
-
- int numEventReceived = 0;
-
- long msg;
- msg = inv_get_message_level_0(1);
- if (msg) {
- if (msg & INV_MSG_MOTION_EVENT) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
- }
- if (msg & INV_MSG_NO_MOTION_EVENT) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
- /* after the first no motion, the gyro should be
- calibrated well */
- mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
- /* if gyros are on and we got a no motion, set a flag
- indicating that the cal file can be written. */
- mHaveGoodMpuCal = true;
- }
- }
-
- // load up virtual sensors
- for (int i = 0; i < numSensors; i++) {
- int update;
- if (mEnabled & (1 << i)) {
- update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
- mPendingMask |= (1 << i);
-
- if (update && (count > 0)) {
- *data++ = mPendingEvents[i];
- count--;
- numEventReceived++;
- }
- }
- }
-
- return numEventReceived;
-}
-
-// collect data for MPL (but NOT sensor service currently), from driver layer
-/* TODO: FIX! data and count are not used, results is hardcoded to 0 */
-/* TODO: This should probably be called "void cacheEvents(void)"
- * And executeOnData() should be int readEvents(data,count)
- */
-int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) {
-
-
- int lp_quaternion_on = 0, nbyte;
- int i, mask = 0, numEventReceived = 0,
- sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
- (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0);
- char *rdata = mIIOBuffer;
-
- nbyte= (8 * sensors + 8) * 1;
-
- if (isLowPowerQuatEnabled()) {
- lp_quaternion_on = checkLPQuaternion();
- if (lp_quaternion_on) {
- nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data
- }
- }
-
- ssize_t rsize = read(iio_fd, rdata, nbyte);
- if (sensors == 0) {
- rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
- }
-
-#ifdef TESTING
- LOGI("get one sample of IIO data with size: %d", rsize);
- LOGI("sensors: %d", sensors);
-
- LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
- *((short *) (rdata + 0)), *((short *) (rdata + 2)),
- *((short *) (rdata + 4)));
- LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
- *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
- *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
- *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
-
- LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
- mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
- *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
- *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
- *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
-#endif
-
- if (rsize != nbyte) {
- LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)",
- rsize, nbyte, sensors, errno, strerror(errno));
- rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
- return -1;
- }
-
- if (isLowPowerQuatEnabled() && lp_quaternion_on) {
-
- for (i=0; i< 4; i++) {
- mCachedQuaternionData[i]= *(long*)rdata;
- rdata += sizeof(long);
- }
- }
-
- for (i = 0; i < 3; i++) {
- if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
- mCachedGyroData[i] = *((short *) (rdata + i * 2));
- }
- if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
- mCachedAccelData[i] = *((short *) (rdata + i * 2 +
- ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
- }
- if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
- mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
- }
- }
-
- mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
- if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
- (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
- mask |= 1 << MagneticField;
- }
-
- mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
- if (mCompassSensor->isIntegrated()) {
- mCompassTimestamp = mSensorTimestamp;
- }
-
- if (mask & (1 << Gyro)) {
- // send down temperature every 0.5 seconds
- // with timestamp measured in "driver" layer
- if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
- mTempCurrentTime = mSensorTimestamp;
- long long temperature[2];
- if(inv_read_temperature(temperature) == 0) {
- LOGV_IF(INPUT_DATA,
- "HAL:inv_read_temperature = %lld, timestamp= %lld",
- temperature[0], temperature[1]);
- inv_build_temp(temperature[0], temperature[1]);
- }
-#ifdef TESTING
- long bias[3], temp, temp_slope[3];
- inv_get_gyro_bias(bias, &temp);
- inv_get_gyro_ts(temp_slope);
-
- LOGI("T: %.3f "
- "GB: %+13f %+13f %+13f "
- "TS: %+13f %+13f %+13f "
- "\n",
- (float)temperature[0] / 65536.f,
- (float)bias[0] / 65536.f / 16.384f,
- (float)bias[1] / 65536.f / 16.384f,
- (float)bias[2] / 65536.f / 16.384f,
- temp_slope[0] / 65536.f,
- temp_slope[1] / 65536.f,
- temp_slope[2] / 65536.f);
-#endif
- }
-
- mPendingMask |= 1 << Gyro;
- mPendingMask |= 1 << RawGyro;
-
- if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
- inv_build_gyro(mCachedGyroData, mSensorTimestamp);
- LOGV_IF(INPUT_DATA,
- "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
- mCachedGyroData[0], mCachedGyroData[1],
- mCachedGyroData[2], mSensorTimestamp);
- }
- }
-
- if (mask & (1 << Accelerometer)) {
- mPendingMask |= 1 << Accelerometer;
- if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
- inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
- LOGV_IF(INPUT_DATA,
- "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
- mCachedAccelData[0], mCachedAccelData[1],
- mCachedAccelData[2], mSensorTimestamp);
- }
- }
-
- if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
- int status = 0;
- if (mCompassSensor->providesCalibration()) {
- status = mCompassSensor->getAccuracy();
- status |= INV_CALIBRATED;
- }
- if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
- inv_build_compass(mCachedCompassData, status,
- mCompassTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
- mCachedCompassData[2], mCompassTimestamp);
- }
- }
-
- if (isLowPowerQuatEnabled() && lp_quaternion_on) {
-
- inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
- mCachedQuaternionData[0], mCachedQuaternionData[1],
- mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
- }
-
- return numEventReceived;
-}
-
-/* use for both MPUxxxx and third party compass */
-int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count)
-{
- VHANDLER_LOG;
-
- if (count < 1)
- return -EINVAL;
-
- int numEventReceived = 0;
- int done = 0;
-
- done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
-#ifdef COMPASS_YAS53x
- if (mCompassSensor->checkCoilsReset()) {
- //Reset relevant compass settings
- resetCompass();
- }
-#endif
- if (done > 0) {
- int status = 0;
- if (mCompassSensor->providesCalibration()) {
- status = mCompassSensor->getAccuracy();
- status |= INV_CALIBRATED;
- }
- if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
- inv_build_compass(mCachedCompassData, status,
- mCompassTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
- mCachedCompassData[2], mCompassTimestamp);
- }
- }
-
- return numEventReceived;
-}
-
-#ifdef COMPASS_YAS53x
-int MPLSensor::resetCompass()
-{
- VFUNC_LOG;
-
- //Reset compass cal if enabled
- if (mFeatureActiveMask & INV_COMPASS_CAL) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
- inv_init_vector_compass_cal();
- }
-
- //Reset compass fit if enabled
- if (mFeatureActiveMask & INV_COMPASS_FIT) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
- inv_init_compass_fit();
- }
-
- return 0;
-}
-#endif
-
-int MPLSensor::getFd() const
-{
- VFUNC_LOG;
- LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
- return iio_fd;
-}
-
-int MPLSensor::getAccelFd() const
-{
- VFUNC_LOG;
- LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
- return accel_fd;
-}
-
-int MPLSensor::getCompassFd() const
-{
- VFUNC_LOG;
- int fd = mCompassSensor->getFd();
- LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
- return fd;
-}
-
-int MPLSensor::turnOffAccelFifo() {
- int i, res;
- char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
- mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
-
- for (i = 0; i < 3; i++) {
- res = write_sysfs_int(accel_fifo_enable[i], 0);
- if (res < 0) {
- return res;
- }
- }
- return 0;
-}
-
-int MPLSensor::enableDmpOrientation(int en)
-{
- VFUNC_LOG;
- /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
- int res = 0;
- int enabled_sensors = mEnabled;
-
- if (isMpu3050())
- return res;
-
- pthread_mutex_lock(&GlobalHalMutex);
-
- // on power if not already On
- res = onPower(1);
- // reset master enable
- res = masterEnable(0);
-
- if (en) {
- //Enable DMP orientation
- if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
- LOGE("HAL:ERR can't enable Android orientation");
- res = -1; // indicate an err
- }
-
- // open DMP Orient Fd
- res = openDmpOrientFd();
-
- // enable DMP
- res = onDMP(1);
-
- // default DMP output rate to FIFO
- if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
- LOGE("HAL:ERR can't default DMP output rate");
- }
-
- // set DMP rate to 200Hz
- if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
- res = -1;
- LOGE("HAL:ERR can't set DMP rate to 200Hz");
- }
-
- // enable accel engine
- res = enableAccel(1);
-
- // disable accel FIFO
- if (!A_ENABLED) {
- res = turnOffAccelFifo();
- }
-
- mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
- } else {
- // disable DMP
- res = onDMP(0);
-
- // disable accel engine
- if (!A_ENABLED) {
- res = enableAccel(0);
- }
- }
-
- res = masterEnable(1);
- pthread_mutex_unlock(&GlobalHalMutex);
- return res;
-}
-
-int MPLSensor::openDmpOrientFd()
-{
- VFUNC_LOG;
-
- if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
- return -1;
- }
-
- dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
- if (dmp_orient_fd < 0) {
- LOGE("HAL:ERR couldn't open dmpOrient node");
- return -1;
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
- return 0;
- }
-}
-
-int MPLSensor::closeDmpOrientFd()
-{
- VFUNC_LOG;
- if (dmp_orient_fd >= 0)
- close(dmp_orient_fd);
- return 0;
-}
-
-int MPLSensor::dmpOrientHandler(int orient)
-{
- VFUNC_LOG;
-
- LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
- return 0;
-}
-
-int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
- VFUNC_LOG;
-
- char dummy[4];
- int screen_orientation = 0;
- FILE *fp;
-
- fp = fopen(mpu.event_display_orientation, "r");
- if (fp == NULL) {
- LOGE("HAL:cannot open event_display_orientation");
- return 0;
- }
- fscanf(fp, "%d\n", &screen_orientation);
- fclose(fp);
-
- int numEventReceived = 0;
-
- if (mDmpOrientationEnabled && count > 0) {
- sensors_event_t temp;
-
- bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */
- temp.version = sizeof(sensors_event_t);
- temp.sensor = ID_SO;
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
- temp.screen_orientation = screen_orientation;
-#endif
- struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC, &ts);
- temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
-
- *data++ = temp;
- count--;
- numEventReceived++;
- }
-
- // read dummy data per driver's request
- dmpOrientHandler(screen_orientation);
- read(dmp_orient_fd, dummy, 4);
-
- return numEventReceived;
-}
-
-int MPLSensor::getDmpOrientFd()
-{
- VFUNC_LOG;
-
- LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
- return dmp_orient_fd;
-
-}
-
-int MPLSensor::checkDMPOrientation()
-{
- VFUNC_LOG;
- return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
-}
-
-int MPLSensor::getDmpRate(int64_t *wanted)
-{
- if (checkDMPOrientation() || checkLPQuaternion()) {
- // set DMP output rate to FIFO
- write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
-
- //DMP running rate must be @ 200Hz
- *wanted= RATE_200HZ;
- LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
- }
- return 0;
-}
-
-int MPLSensor::getPollTime()
-{
- VHANDLER_LOG;
- return mPollTime;
-}
-
-bool MPLSensor::hasPendingEvents() const
-{
- VHANDLER_LOG;
- // if we are using the polling workaround, force the main
- // loop to check for data every time
- return (mPollTime != -1);
-}
-
-/* TODO: support resume suspend when we gain more info about them*/
-void MPLSensor::sleepEvent()
-{
- VFUNC_LOG;
-}
-
-void MPLSensor::wakeEvent()
-{
- VFUNC_LOG;
-}
-
-int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
-{
- VHANDLER_LOG;
-
- if (!fdata || !ldata)
- return -1;
- ldata[0] = (long)(fdata[0] * 65536.f);
- ldata[1] = (long)(fdata[1] * 65536.f);
- ldata[2] = (long)(fdata[2] * 65536.f);
- return 0;
-}
-
-int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
-{
- VHANDLER_LOG;
-
- if (!fdata || !ldata)
- return -1;
- ldata[0] = (fdata[1] * 65536.f);
- ldata[1] = (fdata[2] * 65536.f);
- ldata[2] = (fdata[3] * 65536.f);
- return 0;
-}
-
-int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
-{
- VHANDLER_LOG;
-
- if (!fdata || !ldata)
- return -1;
- ldata[0] = (long)fdata[0];
- ldata[1] = (long)fdata[1];
- ldata[2] = (long)fdata[2];
- return 0;
-}
-
-int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
-{
- VHANDLER_LOG;
-
- if (!fdata || !ldata)
- return -1;
- ldata[0] = (short)fdata[0];
- ldata[1] = (short)fdata[1];
- ldata[2] = (short)fdata[2];
- return 0;
-}
-
-int MPLSensor::inv_read_temperature(long long *data)
-{
- VHANDLER_LOG;
-
- int count = 0;
- char raw_buf[40];
- long raw = 0;
-
- long long timestamp = 0;
-
- memset(raw_buf, 0, sizeof(raw_buf));
- count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
- sizeof(raw_buf));
- if(count < 1) {
- LOGE("HAL:error reading gyro temperature");
- return -1;
- }
-
- count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp);
-
- if(count < 0) {
- return -1;
- }
-
- LOGV_IF(ENG_VERBOSE,
- "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
- raw, timestamp, count);
- data[0] = raw;
- data[1] = timestamp;
-
- return 0;
-}
-
-int MPLSensor::inv_read_dmp_state(int fd)
-{
- VFUNC_LOG;
-
- if(fd < 0)
- return -1;
-
- int count = 0;
- char raw_buf[10];
- short raw = 0;
-
- memset(raw_buf, 0, sizeof(raw_buf));
- count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
- if(count < 1) {
- LOGE("HAL:error reading dmp state");
- close(fd);
- return -1;
- }
- count = sscanf(raw_buf, "%hd", &raw);
- if(count < 0) {
- LOGE("HAL:dmp state data is invalid");
- close(fd);
- return -1;
- }
- LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
- close(fd);
- return (int)raw;
-}
-
-int MPLSensor::inv_read_sensor_bias(int fd, long *data)
-{
- VFUNC_LOG;
-
- if(fd == -1) {
- return -1;
- }
-
- char buf[50];
- char x[15], y[15], z[15];
-
- memset(buf, 0, sizeof(buf));
- int count = read_attribute_sensor(fd, buf, sizeof(buf));
- if(count < 1) {
- LOGE("HAL:Error reading gyro bias");
- return -1;
- }
- count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
- if(count) {
- /* scale appropriately for MPL */
- LOGV_IF(ENG_VERBOSE,
- "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
- atol(x), atol(y), atol(z));
-
- data[0] = (long)(atol(x) / 10000 * (1L << 16));
- data[1] = (long)(atol(y) / 10000 * (1L << 16));
- data[2] = (long)(atol(z) / 10000 * (1L << 16));
-
- LOGV_IF(ENG_VERBOSE,
- "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
- data[0], data[1], data[2]);
- }
- return 0;
-}
-
-/** fill in the sensor list based on which sensors are configured.
- * return the number of configured sensors.
- * parameter list must point to a memory region of at least 7*sizeof(sensor_t)
- * parameter len gives the length of the buffer pointed to by list
- */
-int MPLSensor::populateSensorList(struct sensor_t *list, int len)
-{
- VFUNC_LOG;
-
- int numsensors;
-
- if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
- LOGE("HAL:sensor list too small, not populating.");
- return -(sizeof(sSensorList) / sizeof(sensor_t));
- }
-
- /* fill in the base values */
- memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
-
- /* first add gyro, accel and compass to the list */
-
- /* fill in gyro/accel values */
- fillGyro(chip_ID, list);
- fillAccel(chip_ID, list);
-
- // TODO: need fixes for unified HAL and 3rd-party solution
- mCompassSensor->fillList(&list[MagneticField]);
-
- if(1) {
- numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
- /* all sensors will be added to the list
- fill in orientation values */
- fillOrientation(list);
- /* fill in rotation vector values */
- fillRV(list);
- /* fill in gravity values */
- fillGravity(list);
- /* fill in Linear accel values */
- fillLinearAccel(list);
- } else {
- /* no 9-axis sensors, zero fill that part of the list */
- numsensors = 3;
- memset(list + 3, 0, 4 * sizeof(struct sensor_t));
- }
-
- return numsensors;
-}
-
-void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
-{
- VFUNC_LOG;
-
- if (accel) {
- if(accel != NULL && strcmp(accel, "BMA250") == 0) {
- list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
- list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
- list[Accelerometer].power = ACCEL_BMA250_POWER;
- list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
- return;
- } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
- list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
- list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
- list[Accelerometer].power = ACCEL_MPU6050_POWER;
- list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
- return;
- } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
- list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
- list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
- list[Accelerometer].power = ACCEL_MPU6500_POWER;
- list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
-
- return;
- } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
- list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
- list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
- list[Accelerometer].power = ACCEL_MPU9150_POWER;
- list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
- return;
- } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
- list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
- list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
- list[Accelerometer].power = ACCEL_BMA250_POWER;
- list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
- return;
- }
- }
-
- LOGE("HAL:unknown accel id %s -- "
- "params default to bma250 and might be wrong.",
- accel);
- list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
- list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
- list[Accelerometer].power = ACCEL_BMA250_POWER;
- list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
-}
-
-void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
-{
- VFUNC_LOG;
-
- if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
- list[Gyro].maxRange = GYRO_MPU3050_RANGE;
- list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
- list[Gyro].power = GYRO_MPU3050_POWER;
- list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
- } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
- list[Gyro].maxRange = GYRO_MPU6050_RANGE;
- list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
- list[Gyro].power = GYRO_MPU6050_POWER;
- list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
- } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
- list[Gyro].maxRange = GYRO_MPU6500_RANGE;
- list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
- list[Gyro].power = GYRO_MPU6500_POWER;
- list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
- } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
- list[Gyro].maxRange = GYRO_MPU9150_RANGE;
- list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
- list[Gyro].power = GYRO_MPU9150_POWER;
- list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
- } else {
- LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
- LOGE("HAL:default to use mpu3050 params");
- list[Gyro].maxRange = GYRO_MPU3050_RANGE;
- list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
- list[Gyro].power = GYRO_MPU3050_POWER;
- list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
- }
-
- list[RawGyro].maxRange = list[Gyro].maxRange;
- list[RawGyro].resolution = list[Gyro].resolution;
- list[RawGyro].power = list[Gyro].power;
- list[RawGyro].minDelay = list[Gyro].minDelay;
-
- return;
-}
-
-/* fillRV depends on values of accel and compass in the list */
-void MPLSensor::fillRV(struct sensor_t *list)
-{
- VFUNC_LOG;
-
- /* compute power on the fly */
- list[RotationVector].power = list[Gyro].power +
- list[Accelerometer].power +
- list[MagneticField].power;
- list[RotationVector].resolution = .00001;
- list[RotationVector].maxRange = 1.0;
- list[RotationVector].minDelay = 5000;
-
- return;
-}
-
-void MPLSensor::fillOrientation(struct sensor_t *list)
-{
- VFUNC_LOG;
-
- list[Orientation].power = list[Gyro].power +
- list[Accelerometer].power +
- list[MagneticField].power;
- list[Orientation].resolution = .00001;
- list[Orientation].maxRange = 360.0;
- list[Orientation].minDelay = 5000;
-
- return;
-}
-
-void MPLSensor::fillGravity( struct sensor_t *list)
-{
- VFUNC_LOG;
-
- list[Gravity].power = list[Gyro].power +
- list[Accelerometer].power +
- list[MagneticField].power;
- list[Gravity].resolution = .00001;
- list[Gravity].maxRange = 9.81;
- list[Gravity].minDelay = 5000;
-
- return;
-}
-
-void MPLSensor::fillLinearAccel(struct sensor_t *list)
-{
- VFUNC_LOG;
-
- list[LinearAccel].power = list[Gyro].power +
- list[Accelerometer].power +
- list[MagneticField].power;
- list[LinearAccel].resolution = list[Accelerometer].resolution;
- list[LinearAccel].maxRange = list[Accelerometer].maxRange;
- list[LinearAccel].minDelay = 5000;
-
- return;
-}
-
-int MPLSensor::inv_init_sysfs_attributes(void)
-{
- VFUNC_LOG;
-
- unsigned char i;
- char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
- char *sptr;
- char **dptr;
-
- sysfs_names_ptr =
- (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
- sptr = sysfs_names_ptr;
- if (sptr == NULL) {
- LOGE("HAL:couldn't alloc mem for sysfs paths");
- return -1;
- }
-
- dptr = (char**)&mpu;
- i = 0;
- do {
- *dptr++ = sptr;
- sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
- } while (++i < MAX_SYSFS_ATTRB);
-
- // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
- // inv_get_sysfs_abs_path(sysfs_path);
- if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
- ALOGE("MPLSensor failed get sysfs path");
- return -1;
- }
-
- if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
- ALOGE("MPLSensor failed get iio trigger path");
- return -1;
- }
-
- sprintf(mpu.key, "%s%s", sysfs_path, "/key");
- sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
- sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
- sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
- sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
- sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
- sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
-
- sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
- sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
- sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
- sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
- sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
- sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
- sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
-
- // TODO: for self test
- sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
-
- sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
- sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
- sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
- sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
- sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
- sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
- sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
-
- sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
- sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
- sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
-
-
-#ifndef THIRD_PARTY_ACCEL //MPUxxxx
- sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
- // TODO: for bias settings
- sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
-#endif
-
- sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
- sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
- sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
-
- sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
- sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
- sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
- sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
- sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
-
- sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
- sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
-
-#if SYSFS_VERBOSE
- // test print sysfs paths
- dptr = (char**)&mpu;
- for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
- LOGE("HAL:sysfs path: %s", *dptr++);
- }
-#endif
- return 0;
-}
-
-/* TODO: stop manually testing/using 0 and 1 instead of
- * false and true, but just use 0 and non-0.
- * This allows passing 0 and non-0 ints around instead of
- * having to convert to 1 and test against 1.
- */
-bool MPLSensor::isMpu3050()
-{
- return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
-}
-
-int MPLSensor::isLowPowerQuatEnabled()
-{
-#ifdef ENABLE_LP_QUAT_FEAT
- return !isMpu3050();
-#else
- return 0;
-#endif
-}
-
-int MPLSensor::isDmpDisplayOrientationOn()
-{
-#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
- return !isMpu3050();
-#else
- return 0;
-#endif
-}
diff --git a/60xx/libsensors_iio/MPLSensor.h b/60xx/libsensors_iio/MPLSensor.h
deleted file mode 100644
index 4698f33..0000000
--- a/60xx/libsensors_iio/MPLSensor.h
+++ /dev/null
@@ -1,346 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_MPL_SENSOR_H
-#define ANDROID_MPL_SENSOR_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <poll.h>
-#include <utils/Vector.h>
-#include <utils/KeyedVector.h>
-#include "sensors.h"
-#include "SensorBase.h"
-#include "InputEventReader.h"
-
-#ifdef INVENSENSE_COMPASS_CAL
-
-#ifdef COMPASS_YAS53x
-#pragma message "unified HAL for YAS53x"
-#include "CompassSensor.IIO.primary.h"
-#elif COMPASS_AMI306
-#pragma message "unified HAL for AMI306"
-#include "CompassSensor.IIO.primary.h"
-#else
-#pragma message "unified HAL for MPU9150"
-#include "CompassSensor.IIO.9150.h"
-#endif
-#else
-#pragma message "unified HAL for AKM"
-#include "CompassSensor.AKM.h"
-#endif
-
-/*****************************************************************************/
-/* Sensors Enable/Disable Mask
- *****************************************************************************/
-#define MAX_CHIP_ID_LEN (20)
-
-#define INV_THREE_AXIS_GYRO (0x000F)
-#define INV_THREE_AXIS_ACCEL (0x0070)
-#define INV_THREE_AXIS_COMPASS (0x0380)
-#define INV_ALL_SENSORS (0x7FFF)
-
-#ifdef INVENSENSE_COMPASS_CAL
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
- | INV_THREE_AXIS_COMPASS \
- | INV_THREE_AXIS_GYRO)
-#else
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
- | INV_THREE_AXIS_COMPASS \
- | INV_THREE_AXIS_GYRO)
-#endif
-
-// bit mask of current active features (mFeatureActiveMask)
-#define INV_COMPASS_CAL 0x01
-#define INV_COMPASS_FIT 0x02
-#define INV_DMP_QUATERNION 0x04
-#define INV_DMP_DISPL_ORIENTATION 0x08
-
-/* Uncomment to enable Low Power Quaternion */
-#define ENABLE_LP_QUAT_FEAT
-
-/* Uncomment to enable DMP display orientation
- (within the HAL, see below for Java framework) */
-// #define ENABLE_DMP_DISPL_ORIENT_FEAT
-
-#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
-/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION
- sensor type (DMP screen orientation) to the Java framework.
- NOTE:
- need Invensense customized
- 'hardware/libhardware/include/hardware/sensors.h' to compile correctly.
- NOTE:
- need Invensense java framework changes to:
- 'frameworks/base/core/java/android/view/WindowOrientationListener.java'
- 'frameworks/base/core/java/android/hardware/Sensor.java'
- 'frameworks/base/core/java/android/hardware/SensorEvent.java'
- for the 'Auto-rotate screen' to use this feature.
-*/
-#define ENABLE_DMP_SCREEN_AUTO_ROTATION
-#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly"
-#endif
-
-int isDmpScreenAutoRotationEnabled()
-{
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- return 1;
-#else
- return 0;
-#endif
-}
-
-int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;
-/*****************************************************************************/
-/** MPLSensor implementation which fits into the HAL example for crespo provided
- * by Google.
- * WARNING: there may only be one instance of MPLSensor, ever.
- */
-
-class MPLSensor: public SensorBase
-{
- typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
-
-public:
-
- enum {
- Gyro = 0,
- RawGyro,
- Accelerometer,
- MagneticField,
- Orientation,
- RotationVector,
- LinearAccel,
- Gravity,
- numSensors
- };
-
- MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
- virtual ~MPLSensor();
-
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int enable(int32_t handle, int enabled);
- int32_t getEnableMask() { return mEnabled; }
-
- virtual int readEvents(sensors_event_t *data, int count);
- virtual int getFd() const;
- virtual int getAccelFd() const;
- virtual int getCompassFd() const;
- virtual int getPollTime();
- virtual bool hasPendingEvents() const;
- virtual void sleepEvent();
- virtual void wakeEvent();
- int populateSensorList(struct sensor_t *list, int len);
- void cbProcData();
-
- //static pointer to the object that will handle callbacks
- static MPLSensor* gMPLSensor;
-
- //AKM HAL Integration
- //void set_compass(long ready, long x, long y, long z, long accuracy);
- int executeOnData(sensors_event_t* data, int count);
- int readAccelEvents(sensors_event_t* data, int count);
- int readCompassEvents(sensors_event_t* data, int count);
-
- int turnOffAccelFifo();
- int enableDmpOrientation(int);
- int dmpOrientHandler(int);
- int readDmpOrientEvents(sensors_event_t* data, int count);
- int getDmpOrientFd();
- int openDmpOrientFd();
- int closeDmpOrientFd();
-
- int getDmpRate(int64_t *);
- int checkDMPOrientation();
-
-protected:
- CompassSensor *mCompassSensor;
-
- int gyroHandler(sensors_event_t *data);
- int rawGyroHandler(sensors_event_t *data);
- int accelHandler(sensors_event_t *data);
- int compassHandler(sensors_event_t *data);
- int rvHandler(sensors_event_t *data);
- int laHandler(sensors_event_t *data);
- int gravHandler(sensors_event_t *data);
- int orienHandler(sensors_event_t *data);
- void calcOrientationSensor(float *Rx, float *Val);
- virtual int update_delay();
-
- void inv_set_device_properties();
- int inv_constructor_init();
- int inv_constructor_default_enable();
- int setGyroInitialState();
- int setAccelInitialState();
- int masterEnable(int en);
- int onPower(int en);
- int enableLPQuaternion(int);
- int enableQuaternionData(int);
- int onDMP(int);
- int enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int));
- int enableGyro(int en);
- int enableAccel(int en);
- int enableCompass(int en);
- void computeLocalSensorMask(int enabled_sensors);
- int enableSensors(unsigned long sensors, int en, uint32_t changed);
- int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
- int inv_float_to_q16(float *fdata, long *ldata);
- int inv_long_to_q16(long *fdata, long *ldata);
- int inv_float_to_round(float *fdata, long *ldata);
- int inv_float_to_round2(float *fdata, short *sdata);
- int inv_read_temperature(long long *data);
- int inv_read_dmp_state(int fd);
- int inv_read_sensor_bias(int fd, long *data);
- void inv_get_sensors_orientation(void);
- int inv_init_sysfs_attributes(void);
-#ifdef COMPASS_YAS53x
- int resetCompass(void);
-#endif
- void setCompassDelay(int64_t ns);
- void enable_iio_sysfs(void);
- int enableTap(int);
- int enableFlick(int);
- int enablePedometer(int);
- int checkLPQuaternion();
-
- int mNewData; // flag indicating that the MPL calculated new output values
- int mDmpStarted;
- long mMasterSensorMask;
- long mLocalSensorMask;
- int mPollTime;
- bool mHaveGoodMpuCal; // flag indicating that the cal file can be written
- int mGyroAccuracy; // value indicating the quality of the gyro calibr.
- int mAccelAccuracy; // value indicating the quality of the accel calibr.
- int mCompassAccuracy; // value indicating the quality of the compass calibr.
- struct pollfd mPollFds[5];
- int mSampleCount;
- pthread_mutex_t mMplMutex;
- pthread_mutex_t mHALMutex;
-
- char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
-
- int iio_fd;
- int accel_fd;
- int mpufifo_fd;
- int gyro_temperature_fd;
- int dmp_orient_fd;
-
- int mDmpOrientationEnabled;
-
-
- uint32_t mEnabled;
- uint32_t mOldEnabledMask;
- sensors_event_t mPendingEvents[numSensors];
- int64_t mDelays[numSensors];
- hfunc_t mHandlers[numSensors];
- short mCachedGyroData[3];
- long mCachedAccelData[3];
- long mCachedCompassData[3];
- long mCachedQuaternionData[4];
- android::KeyedVector<int, int> mIrqFds;
-
- InputEventCircularReader mAccelInputReader;
- InputEventCircularReader mGyroInputReader;
-
- bool mFirstRead;
- short mTempScale;
- short mTempOffset;
- int64_t mTempCurrentTime;
- int mAccelScale;
-
- uint32_t mPendingMask;
- unsigned long mSensorMask;
-
- char chip_ID[MAX_CHIP_ID_LEN];
-
- signed char mGyroOrientation[9];
- signed char mAccelOrientation[9];
-
- int64_t mSensorTimestamp;
- int64_t mCompassTimestamp;
-
- struct sysfs_attrbs {
- char *chip_enable;
- char *power_state;
- char *dmp_firmware;
- char *firmware_loaded;
- char *dmp_on;
- char *dmp_int_on;
- char *dmp_event_int_on;
- char *dmp_output_rate;
- char *tap_on;
- char *key;
- char *self_test;
- char *temperature;
-
- char *gyro_enable;
- char *gyro_fifo_rate;
- char *gyro_orient;
- char *gyro_x_fifo_enable;
- char *gyro_y_fifo_enable;
- char *gyro_z_fifo_enable;
-
- char *accel_enable;
- char *accel_fifo_rate;
- char *accel_fsr;
- char *accel_bias;
- char *accel_orient;
- char *accel_x_fifo_enable;
- char *accel_y_fifo_enable;
- char *accel_z_fifo_enable;
-
- char *quaternion_on;
- char *in_quat_r_en;
- char *in_quat_x_en;
- char *in_quat_y_en;
- char *in_quat_z_en;
-
- char *in_timestamp_en;
- char *trigger_name;
- char *current_trigger;
- char *buffer_length;
-
- char *display_orientation_on;
- char *event_display_orientation;
- } mpu;
-
- char *sysfs_names_ptr;
- int mFeatureActiveMask;
-
-private:
- /* added for dynamic get sensor list */
- void fillAccel(const char* accel, struct sensor_t *list);
- void fillGyro(const char* gyro, struct sensor_t *list);
- void fillRV(struct sensor_t *list);
- void fillOrientation(struct sensor_t *list);
- void fillGravity(struct sensor_t *list);
- void fillLinearAccel(struct sensor_t *list);
- void storeCalibration();
- void loadDMP();
- bool isMpu3050();
- int isLowPowerQuatEnabled();
- int isDmpDisplayOrientationOn();
-
-
-};
-
-extern "C" {
- void setCallbackObject(MPLSensor*);
- MPLSensor *getCallbackObject();
-}
-
-#endif // ANDROID_MPL_SENSOR_H
diff --git a/60xx/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp
deleted file mode 100644
index 9773562..0000000
--- a/60xx/libsensors_iio/MPLSupport.cpp
+++ /dev/null
@@ -1,190 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define LOG_NDEBUG 0
-
-#include <MPLSupport.h>
-#include <string.h>
-#include <stdio.h>
-#include <fcntl.h>
-
-#include "log.h"
-#include "SensorBase.h"
-
-#include "ml_sysfs_helper.h"
-#include "local_log_def.h"
-
-int64_t getTimestamp()
-{
- struct timespec t;
- t.tv_sec = t.tv_nsec = 0;
- clock_gettime(CLOCK_MONOTONIC, &t);
- return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
-}
-
-int64_t timevalToNano(timeval const& t) {
- return t.tv_sec * 1000000000LL + t.tv_usec * 1000;
-}
-
-int inv_read_data(char *fname, long *data)
-{
- VFUNC_LOG;
-
- char buf[sizeof(long) * 4];
- int count, fd;
-
- fd = open(fname, O_RDONLY);
- if(fd < 0) {
- LOGE("HAL:Error opening %s", fname);
- return -1;
- }
- memset(buf, 0, sizeof(buf));
- count = read_attribute_sensor(fd, buf, sizeof(buf));
- if(count < 1) {
- close(fd);
- return -1;
- } else {
- count = sscanf(buf, "%ld", data);
- if(count)
- LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
- }
- close(fd);
-
- return 0;
-}
-
-/* This one DOES NOT close FDs for you */
-int read_attribute_sensor(int fd, char* data, unsigned int size)
-{
- VFUNC_LOG;
-
- int count = 0;
- if (fd > 0) {
- count = pread(fd, data, size, 0);
- if(count < 1) {
- LOGE("HAL:read fails with error code=%d", count);
- }
- }
- return count;
-}
-
-/**
- * @brief Enable a sensor through the sysfs file descriptor
- * provided.
- * @note this function one closes FD after the write
- * @param fd
- * the file descriptor to write into
- * @param en
- * the value to write, typically 1 or 0
- * @return the errno whenever applicable.
- */
-int enable_sysfs_sensor(int fd, int en)
-{
- VFUNC_LOG;
-
- int nb;
- int err = 0;
-
- char c = en ? '1' : '0';
- nb = write(fd, &c, 1);
-
- if (nb <= 0) {
- err = errno;
- LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
- c, nb, strerror(err), err);
- }
- close(fd);
-
-
- return err;
-}
-
-/* This one closes FDs for you */
-int write_attribute_sensor(int fd, long data)
-{
- VFUNC_LOG;
-
- int num_b = 0;
-
- if (fd >= 0) {
- char buf[80];
- sprintf(buf, "%ld", data);
- num_b = write(fd, buf, strlen(buf) + 1);
- if (num_b <= 0) {
- int err = errno;
- LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
- } else {
- LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
- }
- close(fd);
- }
-
- return num_b;
-}
-
-int read_sysfs_int(char *filename, int *var)
-{
- int res=0;
- FILE *sysfsfp;
-
- sysfsfp = fopen(filename, "r");
- if (sysfsfp != NULL) {
- if (fscanf(sysfsfp, "%d\n", var) < 1) {
- LOGE("HAL:ERR failed to read an int from %s.", filename);
- res = -EINVAL;
- }
- fclose(sysfsfp);
- } else {
- res = -errno;
- LOGE("HAL:ERR open file %s to read with error %d", filename, res);
- }
- return res;
-}
-
-int write_sysfs_int(char *filename, int var)
-{
- int res = 0;
- FILE *sysfsfp;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- var, filename, getTimestamp());
- sysfsfp = fopen(filename, "w");
- if (sysfsfp == NULL) {
- res = -errno;
- LOGE("HAL:ERR open file %s to write with error %d", filename, res);
- return res;
- }
- int fpres, fcres = 0;
- fpres = fprintf(sysfsfp, "%d\n", var);
- /* fprintf() can succeed because it never actually writes to the
- * underlying sysfs node.
- */
- if (fpres < 0) {
- res = -errno;
- fclose(sysfsfp);
- } else {
- fcres = fclose(sysfsfp);
- /* Check for errors from: fflush(), write(), and close() */
- if (fcres < 0) {
- res = -errno;
- }
- }
- if (fpres < 0 || fcres < 0) {
- LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d",
- var, filename, res, fpres, fcres);
- }
- return res;
-}
diff --git a/60xx/libsensors_iio/MPLSupport.h b/60xx/libsensors_iio/MPLSupport.h
deleted file mode 100644
index 98e4497..0000000
--- a/60xx/libsensors_iio/MPLSupport.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_MPL_SUPPORT_H
-#define ANDROID_MPL_SUPPORT_H
-
-#include <stdint.h>
-#include <time.h>
-
-int64_t getTimestamp();
-int64_t timevalToNano(timeval const& t);
-
-int inv_read_data(char *fname, long *data);
-int read_attribute_sensor(int fd, char* data, unsigned int size);
-int enable_sysfs_sensor(int fd, int en);
-int write_attribute_sensor(int fd, long data);
-int read_sysfs_int(char*, int*);
-int write_sysfs_int(char*, int);
-
-#endif // ANDROID_MPL_SUPPORT_H
diff --git a/60xx/libsensors_iio/SensorBase.cpp b/60xx/libsensors_iio/SensorBase.cpp
deleted file mode 100644
index 587aaf5..0000000
--- a/60xx/libsensors_iio/SensorBase.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <dirent.h>
-#include <string.h>
-#include <sys/select.h>
-#include <cutils/log.h>
-#include <linux/input.h>
-
-#include "SensorBase.h"
-#include "local_log_def.h"
-
-/*****************************************************************************/
-
-SensorBase::SensorBase(const char* dev_name,
- const char* data_name) : dev_name(dev_name),
- data_name(data_name),
- dev_fd(-1),
- data_fd(-1)
-{
- if (data_name) {
- data_fd = openInput(data_name);
- }
-}
-
-SensorBase::~SensorBase()
-{
- if (data_fd >= 0) {
- close(data_fd);
- }
- if (dev_fd >= 0) {
- close(dev_fd);
- }
-}
-
-int SensorBase::open_device()
-{
- if (dev_fd<0 && dev_name) {
- dev_fd = open(dev_name, O_RDONLY);
- LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
- }
- return 0;
-}
-
-int SensorBase::close_device()
-{
- if (dev_fd >= 0) {
- close(dev_fd);
- dev_fd = -1;
- }
- return 0;
-}
-
-int SensorBase::getFd() const
-{
- if (!data_name) {
- return dev_fd;
- }
- return data_fd;
-}
-
-int SensorBase::setDelay(int32_t /*handle*/, int64_t /*ns*/)
-{
- return 0;
-}
-
-bool SensorBase::hasPendingEvents() const
-{
- return false;
-}
-
-int SensorBase::openInput(const char *inputName)
-{
- int fd = -1;
- const char *dirname = "/dev/input";
- char devname[PATH_MAX];
- char *filename;
- DIR *dir;
- struct dirent *de;
- dir = opendir(dirname);
- if(dir == NULL)
- return -1;
- strcpy(devname, dirname);
- filename = devname + strlen(devname);
- *filename++ = '/';
- while((de = readdir(dir))) {
- if(de->d_name[0] == '.' &&
- (de->d_name[1] == '\0' ||
- (de->d_name[1] == '.' && de->d_name[2] == '\0')))
- continue;
- strcpy(filename, de->d_name);
- fd = open(devname, O_RDONLY);
- LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
- LOGI("path open %s", devname);
- if (fd >= 0) {
- char name[80];
- if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
- name[0] = '\0';
- }
- LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
- if (!strcmp(name, inputName)) {
- strcpy(input_name, filename);
- break;
- } else {
- close(fd);
- fd = -1;
- }
- }
- }
- closedir(dir);
- LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
- return fd;
-}
-
-int SensorBase::enable(int32_t /*handle*/, int /*enabled*/)
-{
- return 0;
-}
diff --git a/60xx/libsensors_iio/SensorBase.h b/60xx/libsensors_iio/SensorBase.h
deleted file mode 100644
index fef47c7..0000000
--- a/60xx/libsensors_iio/SensorBase.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_SENSOR_BASE_H
-#define ANDROID_SENSOR_BASE_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember))
-
-#define MAX_SYSFS_NAME_LEN (100)
-#define IIO_BUFFER_LENGTH (480)
-
-/*****************************************************************************/
-
-struct sensors_event_t;
-
-class SensorBase {
-protected:
- const char *dev_name;
- const char *data_name;
- char input_name[PATH_MAX];
- int dev_fd;
- int data_fd;
-
- int openInput(const char* inputName);
-
- int open_device();
- int close_device();
-
-public:
- SensorBase(const char* dev_name, const char* data_name);
-
- virtual ~SensorBase();
-
- virtual int readEvents(sensors_event_t* data, int count) = 0;
- virtual bool hasPendingEvents() const;
- virtual int getFd() const;
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int enable(int32_t handle, int enabled);
-};
-
-/*****************************************************************************/
-
-#endif // ANDROID_SENSOR_BASE_H
diff --git a/60xx/libsensors_iio/libmllite.so b/60xx/libsensors_iio/libmllite.so
deleted file mode 100755
index 9bdd5bc..0000000
--- a/60xx/libsensors_iio/libmllite.so
+++ /dev/null
Binary files differ
diff --git a/60xx/libsensors_iio/libmplmpu.so b/60xx/libsensors_iio/libmplmpu.so
deleted file mode 100644
index 205ab9a..0000000
--- a/60xx/libsensors_iio/libmplmpu.so
+++ /dev/null
Binary files differ
diff --git a/60xx/libsensors_iio/local_log_def.h b/60xx/libsensors_iio/local_log_def.h
deleted file mode 100644
index 9135992..0000000
--- a/60xx/libsensors_iio/local_log_def.h
+++ /dev/null
@@ -1,49 +0,0 @@
-#ifndef LOCAL_LOG_DEF_H
-#define LOCAL_LOG_DEF_H
-
-/* Log enablers, each of these independent */
-
-#define PROCESS_VERBOSE (0) /* process log messages */
-#define EXTRA_VERBOSE (0) /* verbose log messages */
-#define SYSFS_VERBOSE (0) /* log sysfs interactions as cat/echo for repro
- purpose on a shell */
-#define FUNC_ENTRY (0) /* log entry in all one-time functions */
-
-/* Note that enabling this logs may affect performance */
-#define HANDLER_ENTRY (0) /* log entry in all handler functions */
-#define ENG_VERBOSE (0) /* log some a lot more info about the internals */
-#define INPUT_DATA (0) /* log the data input from the events */
-#define HANDLER_DATA (0) /* log the data fetched from the handlers */
-
-#if defined ANDROID_JELLYBEAN
-#define LOGV ALOGV
-#define LOGV_IF ALOGV_IF
-#define LOGD ALOGD
-#define LOGD_IF ALOGD_IF
-#define LOGI ALOGI
-#define LOGI_IF ALOGI_IF
-#define LOGW ALOGW
-#define LOGW_IF ALOGW_IF
-#define LOGE ALOGE
-#define LOGE_IF ALOGE_IF
-#define IF_LOGV IF_ALOGV
-#define IF_LOGD IF_ALOGD
-#define IF_LOGI IF_ALOGI
-#define IF_LOGW IF_ALOGW
-#define IF_LOGE IF_ALOGE
-#define LOG_ASSERT ALOG_ASSERT
-#define LOG ALOG
-#define IF_LOG IF_ALOG
-#else
-#warning "build for ICS or earlier version"
-#endif
-
-
-#define FUNC_LOG \
- LOGV("%s", __PRETTY_FUNCTION__)
-#define VFUNC_LOG \
- LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)
-#define VHANDLER_LOG \
- LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
-
-#endif /*ifndef LOCAL_LOG_DEF_H */
diff --git a/60xx/libsensors_iio/sensor_params.h b/60xx/libsensors_iio/sensor_params.h
deleted file mode 100644
index eef0b3b..0000000
--- a/60xx/libsensors_iio/sensor_params.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef INV_SENSOR_PARAMS_H
-#define INV_SENSOR_PARAMS_H
-
-/* Physical parameters of the sensors supported by Invensense MPL */
-#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV)
-#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA)
-#define SENSORS_GRAVITY_HANDLE (ID_GR)
-#define SENSORS_GYROSCOPE_HANDLE (ID_GY)
-#define SENSORS_RAW_GYROSCOPE_HANDLE (ID_RG)
-#define SENSORS_ACCELERATION_HANDLE (ID_A)
-#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M)
-#define SENSORS_ORIENTATION_HANDLE (ID_O)
-#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO)
-
-/******************************************/
-//MPU9250 INV_COMPASS
-#define COMPASS_MPU9250_RANGE (9830.f)
-#define COMPASS_MPU9250_RESOLUTION (0.15f)
-#define COMPASS_MPU9250_POWER (10.f)
-#define COMPASS_MPU9250_MINDELAY (10000)
-//MPU9150 INV_COMPASS
-#define COMPASS_MPU9150_RANGE (9830.f)
-#define COMPASS_MPU9150_RESOLUTION (0.285f)
-#define COMPASS_MPU9150_POWER (10.f)
-#define COMPASS_MPU9150_MINDELAY (10000)
-//COMPASS_ID_AK8975
-#define COMPASS_AKM8975_RANGE (9830.f)
-#define COMPASS_AKM8975_RESOLUTION (0.285f)
-#define COMPASS_AKM8975_POWER (10.f)
-#define COMPASS_AKM8975_MINDELAY (10000)
-//COMPASS_ID_AK8963C
-#define COMPASS_AKM8963_RANGE (9830.f)
-#define COMPASS_AKM8963_RESOLUTION (0.15f)
-#define COMPASS_AKM8963_POWER (10.f)
-#define COMPASS_AKM8963_MINDELAY (10000)
-//COMPASS_ID_AMI30X
-#define COMPASS_AMI30X_RANGE (5461.f)
-#define COMPASS_AMI30X_RESOLUTION (0.9f)
-#define COMPASS_AMI30X_POWER (0.15f)
-//COMPASS_ID_AMI306
-#define COMPASS_AMI306_RANGE (5461.f)
-#define COMPASS_AMI306_RESOLUTION (0.9f)
-#define COMPASS_AMI306_POWER (0.15f)
-#define COMPASS_AMI306_MINDELAY (10000)
-//COMPASS_ID_YAS529
-#define COMPASS_YAS529_RANGE (19660.f)
-#define COMPASS_YAS529_RESOLUTION (0.012f)
-#define COMPASS_YAS529_POWER (4.f)
-//COMPASS_ID_YAS53x
-#define COMPASS_YAS53x_RANGE (8001.f)
-#define COMPASS_YAS53x_RESOLUTION (0.012f)
-#define COMPASS_YAS53x_POWER (4.f)
-#define COMPASS_YAS53x_MINDELAY (10000)
-//COMPASS_ID_HMC5883
-#define COMPASS_HMC5883_RANGE (10673.f)
-#define COMPASS_HMC5883_RESOLUTION (10.f)
-#define COMPASS_HMC5883_POWER (0.24f)
-//COMPASS_ID_LSM303DLH
-#define COMPASS_LSM303DLH_RANGE (10240.f)
-#define COMPASS_LSM303DLH_RESOLUTION (1.f)
-#define COMPASS_LSM303DLH_POWER (1.f)
-//COMPASS_ID_LSM303DLM
-#define COMPASS_LSM303DLM_RANGE (10240.f)
-#define COMPASS_LSM303DLM_RESOLUTION (1.f)
-#define COMPASS_LSM303DLM_POWER (1.f)
-//COMPASS_ID_MMC314X
-#define COMPASS_MMC314X_RANGE (400.f)
-#define COMPASS_MMC314X_RESOLUTION (2.f)
-#define COMPASS_MMC314X_POWER (0.55f)
-//COMPASS_ID_HSCDTD002B
-#define COMPASS_HSCDTD002B_RANGE (9830.f)
-#define COMPASS_HSCDTD002B_RESOLUTION (1.f)
-#define COMPASS_HSCDTD002B_POWER (1.f)
-//COMPASS_ID_HSCDTD004A
-#define COMPASS_HSCDTD004A_RANGE (9830.f)
-#define COMPASS_HSCDTD004A_RESOLUTION (1.f)
-#define COMPASS_HSCDTD004A_POWER (1.f)
-/*******************************************/
-//ACCEL_ID_MPU6500
-#define ACCEL_MPU6500_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU6500_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU6500_POWER (0.f)
-#define ACCEL_MPU6500_MINDELAY (1000)
-//ACCEL_ID_MPU9250
-#define ACCEL_MPU9250_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU9250_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU9250_POWER (0.f)
-#define ACCEL_MPU9250_MINDELAY (1000)
-//ACCEL_ID_MPU9150
-#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU9150_POWER (0.f)
-#define ACCEL_MPU9150_MINDELAY (1000)
-//ACCEL_ID_LIS331
-#define ACCEL_LIS331_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LIS331_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LIS331_POWER (1.f)
-//ACCEL_ID_LSM303DLX
-#define ACCEL_LSM303DLX_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_POWER (1.f)
-//ACCEL_ID_LIS3DH
-#define ACCEL_LIS3DH_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LIS3DH_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LIS3DH_POWER (1.f)
-//ACCEL_ID_KXSD9
-#define ACCEL_KXSD9_RANGE (2.5006f * GRAVITY_EARTH)
-#define ACCEL_KXSD9_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_KXSD9_POWER (1.f)
-//ACCEL_ID_KXTF9
-#define ACCEL_KXTF9_RANGE (1.f * GRAVITY_EARTH)
-#define ACCEL_KXTF9_RESOLUTION (0.033f * GRAVITY_EARTH)
-#define ACCEL_KXTF9_POWER (0.35f)
-//ACCEL_ID_BMA150
-#define ACCEL_BMA150_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA150_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_BMA150_POWER (0.2f)
-//ACCEL_ID_BMA222
-#define ACCEL_BMA222_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA222_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_BMA222_POWER (0.1f)
-//ACCEL_ID_BMA250
-#define ACCEL_BMA250_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA250_RESOLUTION (0.00391f * GRAVITY_EARTH)
-#define ACCEL_BMA250_POWER (0.139f)
-#define ACCEL_BMA250_MINDELAY (1000)
-//ACCEL_ID_ADXL34X
-#define ACCEL_ADXL34X_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_ADXL34X_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_ADXL34X_POWER (1.f)
-//ACCEL_ID_MMA8450
-#define ACCEL_MMA8450_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MMA8450_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_MMA8450_POWER (1.0f)
-//ACCEL_ID_MMA845X
-#define ACCEL_MMA845X_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MMA845X_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_MMA845X_POWER (1.f)
-//ACCEL_ID_MPU6050
-#define ACCEL_MPU6050_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU6050_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU6050_POWER (0.f)
-#define ACCEL_MPU6050_MINDELAY (5000)
-/******************************************/
-//GYRO MPU3050
-#define RAD_P_DEG (3.14159f / 180.f)
-#define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU3050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU3050_POWER (6.1f)
-#define GYRO_MPU3050_MINDELAY (1000)
-//GYRO MPU6050
-#define GYRO_MPU6050_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU6050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU6050_POWER (5.5f)
-#define GYRO_MPU6050_MINDELAY (5000)
-//GYRO MPU9150
-#define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU9150_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU9150_POWER (5.5f)
-#define GYRO_MPU9150_MINDELAY (1000)
-//GYRO MPU9250
-#define GYRO_MPU9250_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU9250_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU9250_POWER (5.5f)
-#define GYRO_MPU9250_MINDELAY (1000)
-//GYRO MPU6500
-#define GYRO_MPU6500_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU6500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU6500_POWER (5.5f)
-#define GYRO_MPU6500_MINDELAY (1000)
-//GYRO ITG3500
-#define GYRO_ITG3500_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_ITG3500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_ITG3500_POWER (5.5f)
-#define GYRO_ITG3500_MINDELAY (1000)
-
-#endif /* INV_SENSOR_PARAMS_H */
-
diff --git a/60xx/libsensors_iio/sensors.h b/60xx/libsensors_iio/sensors.h
deleted file mode 100644
index f698fc5..0000000
--- a/60xx/libsensors_iio/sensors.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_SENSORS_H
-#define ANDROID_SENSORS_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <hardware/hardware.h>
-#include <hardware/sensors.h>
-
-__BEGIN_DECLS
-
-/*****************************************************************************/
-
-#ifndef ARRAY_SIZE
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
-#endif
-
-enum {
- ID_GY = 0,
- ID_RG,
- ID_A,
- ID_M,
- ID_O,
- ID_RV,
- ID_LA,
- ID_GR,
- ID_SO
-};
-
-/*****************************************************************************/
-
-/*
- * The SENSORS Module
- */
-
-/* ITG3500 */
-#define EVENT_TYPE_GYRO_X REL_X
-#define EVENT_TYPE_GYRO_Y REL_Y
-#define EVENT_TYPE_GYRO_Z REL_Z
-/* MPU6050 MPU9150 */
-#define EVENT_TYPE_IACCEL_X REL_RX
-#define EVENT_TYPE_IACCEL_Y REL_RY
-#define EVENT_TYPE_IACCEL_Z REL_RZ
-/* MPU6050 MPU9150 */
-#define EVENT_TYPE_ICOMPASS_X REL_X
-#define EVENT_TYPE_ICOMPASS_Y REL_Y
-#define EVENT_TYPE_ICOMPASS_Z REL_Z
-/* MPUxxxx */
-#define EVENT_TYPE_TIMESTAMP_HI REL_MISC
-#define EVENT_TYPE_TIMESTAMP_LO REL_WHEEL
-
-/* Accel BMA250 */
-#define EVENT_TYPE_ACCEL_X ABS_X
-#define EVENT_TYPE_ACCEL_Y ABS_Y
-#define EVENT_TYPE_ACCEL_Z ABS_Z
-#define LSG (1000.0f)
-
-// conversion of acceleration data to SI units (m/s^2)
-#define RANGE_A (4*GRAVITY_EARTH)
-#define RESOLUTION_A (GRAVITY_EARTH / LSG)
-#define CONVERT_A (GRAVITY_EARTH / LSG)
-#define CONVERT_A_X (CONVERT_A)
-#define CONVERT_A_Y (CONVERT_A)
-#define CONVERT_A_Z (CONVERT_A)
-
-/* Compass AKM8975 */
-#define EVENT_TYPE_MAGV_X ABS_RX
-#define EVENT_TYPE_MAGV_Y ABS_RY
-#define EVENT_TYPE_MAGV_Z ABS_RZ
-#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER
-
-// conversion of magnetic data to uT units
-#define CONVERT_M (0.06f)
-
-__END_DECLS
-
-#endif // ANDROID_SENSORS_H
diff --git a/60xx/libsensors_iio/sensors_mpl.cpp b/60xx/libsensors_iio/sensors_mpl.cpp
deleted file mode 100644
index 4aef514..0000000
--- a/60xx/libsensors_iio/sensors_mpl.cpp
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
-
-#include <hardware/sensors.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <dirent.h>
-#include <math.h>
-#include <poll.h>
-#include <pthread.h>
-#include <stdlib.h>
-
-#include <linux/input.h>
-
-#include <utils/Atomic.h>
-#include <utils/Log.h>
-
-#include "sensors.h"
-#include "MPLSensor.h"
-
-/*****************************************************************************/
-/* The SENSORS Module */
-
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
-#define LOCAL_SENSORS (MPLSensor::numSensors + 1)
-#else
-#define LOCAL_SENSORS MPLSensor::numSensors
-#endif
-
-/* Vendor-defined Accel Load Calibration File Method
-* @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame
-* @return '0' for a successful load, '1' otherwise
-* example: int AccelLoadConfig(long* offset);
-* End of Vendor-defined Accel Load Cal Method
-*/
-
-static struct sensor_t sSensorList[LOCAL_SENSORS];
-static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
-
-static int open_sensors(const struct hw_module_t* module, const char* id,
- struct hw_device_t** device);
-
-static int sensors__get_sensors_list(struct sensors_module_t* module,
- struct sensor_t const** list)
-{
- *list = sSensorList;
- return sensors;
-}
-
-static struct hw_module_methods_t sensors_module_methods = {
- open: open_sensors
-};
-
-struct sensors_module_t HAL_MODULE_INFO_SYM = {
- common: {
- tag: HARDWARE_MODULE_TAG,
- version_major: 1,
- version_minor: 0,
- id: SENSORS_HARDWARE_MODULE_ID,
- name: "Invensense module",
- author: "Invensense Inc.",
- methods: &sensors_module_methods,
- },
- get_sensors_list: sensors__get_sensors_list,
-};
-
-struct sensors_poll_context_t {
- struct sensors_poll_device_t device; // must be first
-
- sensors_poll_context_t();
- ~sensors_poll_context_t();
- int activate(int handle, int enabled);
- int setDelay(int handle, int64_t ns);
- int pollEvents(sensors_event_t* data, int count);
-
-private:
- enum {
- mpl = 0,
- compass,
- dmpOrient,
- numSensorDrivers, // wake pipe goes here
- numFds,
- };
-
- struct pollfd mPollFds[numSensorDrivers];
- SensorBase *mSensor;
-};
-
-/******************************************************************************/
-
-sensors_poll_context_t::sensors_poll_context_t() {
- VFUNC_LOG;
-
- mCompassSensor = new CompassSensor();
- MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
-
- /* For Vendor-defined Accel Calibration File Load
- * Use the Following Constructor and Pass Your Load Cal File Function
- *
- * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
- */
-
- // setup the callback object for handing mpl callbacks
- setCallbackObject(mplSensor);
-
- // populate the sensor list
- sensors =
- mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
-
- mSensor = mplSensor;
- mPollFds[mpl].fd = mSensor->getFd();
- mPollFds[mpl].events = POLLIN;
- mPollFds[mpl].revents = 0;
-
- mPollFds[compass].fd = mCompassSensor->getFd();
- mPollFds[compass].events = POLLIN;
- mPollFds[compass].revents = 0;
-
- mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
- mPollFds[dmpOrient].events = POLLPRI;
- mPollFds[dmpOrient].revents = 0;
-}
-
-sensors_poll_context_t::~sensors_poll_context_t() {
- FUNC_LOG;
- delete mSensor;
-}
-
-int sensors_poll_context_t::activate(int handle, int enabled) {
- FUNC_LOG;
- return mSensor->enable(handle, enabled);
-}
-
-int sensors_poll_context_t::setDelay(int handle, int64_t ns)
-{
- FUNC_LOG;
- return mSensor->setDelay(handle, ns);
-}
-
-int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
-{
- VHANDLER_LOG;
-
- int nbEvents = 0;
- int nb, polltime = -1;
-
- // look for new events
- nb = poll(mPollFds, numSensorDrivers, polltime);
-
- if (nb > 0) {
- for (int i = 0; count && i < numSensorDrivers; i++) {
- if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
- nb = 0;
- if (i == mpl) {
- /* Ignore res */
- mSensor->readEvents(NULL, 0);
- mPollFds[i].revents = 0;
- }
- else if (i == compass) {
- /* Ignore res */
- ((MPLSensor*) mSensor)->readCompassEvents(NULL, count);
- mPollFds[i].revents = 0;
- }
- }
- }
- nb = ((MPLSensor*) mSensor)->executeOnData(data, count);
- if (nb > 0) {
- count -= nb;
- nbEvents += nb;
- data += nb;
- }
-
- if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {
- nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);
- mPollFds[dmpOrient].revents= 0;
- if (isDmpScreenAutoRotationEnabled() && nb > 0) {
- count -= nb;
- nbEvents += nb;
- data += nb;
- }
- }
- }
-
- return nbEvents;
-}
-
-/******************************************************************************/
-
-static int poll__close(struct hw_device_t *dev)
-{
- FUNC_LOG;
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- if (ctx) {
- delete ctx;
- }
- return 0;
-}
-
-static int poll__activate(struct sensors_poll_device_t *dev,
- int handle, int enabled)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- return ctx->activate(handle, enabled);
-}
-
-static int poll__setDelay(struct sensors_poll_device_t *dev,
- int handle, int64_t ns)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- int s= ctx->setDelay(handle, ns);
- return s;
-}
-
-static int poll__poll(struct sensors_poll_device_t *dev,
- sensors_event_t* data, int count)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- return ctx->pollEvents(data, count);
-}
-
-/******************************************************************************/
-
-/** Open a new instance of a sensor device using name */
-static int open_sensors(const struct hw_module_t* module, const char* id,
- struct hw_device_t** device)
-{
- FUNC_LOG;
- int status = -EINVAL;
- sensors_poll_context_t *dev = new sensors_poll_context_t();
-
- memset(&dev->device, 0, sizeof(sensors_poll_device_t));
-
- dev->device.common.tag = HARDWARE_DEVICE_TAG;
- dev->device.common.version = 0;
- dev->device.common.module = const_cast<hw_module_t*>(module);
- dev->device.common.close = poll__close;
- dev->device.activate = poll__activate;
- dev->device.setDelay = poll__setDelay;
- dev->device.poll = poll__poll;
-
- *device = &dev->device.common;
- status = 0;
-
- return status;
-}
diff --git a/60xx/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk
deleted file mode 100644
index 84e7e9b..0000000
--- a/60xx/libsensors_iio/software/build/android/common.mk
+++ /dev/null
@@ -1,87 +0,0 @@
-# Use bash for additional echo fancyness
-SHELL = /bin/bash
-
-####################################################################################################
-## defines
-
-# Build for Jellybean
-BUILD_ANDROID_JELLYBEAN = 1
-
-## libraries ##
-LIB_PREFIX = lib
-
-STATIC_LIB_EXT = a
-SHARED_LIB_EXT = so
-
-# normally, overridden from outside
-# ?= assignment sets it only if not already defined
-TARGET ?= android
-
-MLLITE_LIB_NAME ?= mllite
-MPL_LIB_NAME ?= mplmpu
-
-## applications ##
-SHARED_APP_SUFFIX = -shared
-STATIC_APP_SUFFIX = -static
-
-####################################################################################################
-## compile, includes, and linker
-
-ifeq ($(BUILD_ANDROID_JELLYBEAN),1)
-ANDROID_COMPILE = -DANDROID_JELLYBEAN=1
-endif
-
-ANDROID_LINK = -nostdlib
-ANDROID_LINK += -fpic
-ANDROID_LINK += -Wl,--gc-sections
-ANDROID_LINK += -Wl,--no-whole-archive
-ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib
-ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
-
-ANDROID_LINK_EXECUTABLE = $(ANDROID_LINK)
-ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker
-ifneq ($(BUILD_ANDROID_JELLYBEAN),1)
-ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x
-endif
-ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o
-ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o
-
-ANDROID_INCLUDES = -I$(ANDROID_ROOT)/system/core/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include # ICS
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm
-ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates
-
-KERNEL_INCLUDES = -I$(KERNEL_ROOT)/include
-
-INV_INCLUDES = -I$(INV_ROOT)/software/core/driver/include
-INV_INCLUDES += -I$(MLLITE_DIR)
-INV_INCLUDES += -I$(MLLITE_DIR)/linux
-
-INV_DEFINES += -DINV_CACHE_DMP=1
-
-####################################################################################################
-## macros
-
-ifndef echo_in_colors
-define echo_in_colors
- echo -ne "\e[1;32m"$(1)"\e[0m"
-endef
-endif
-
-
-
diff --git a/60xx/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk
deleted file mode 100644
index 47dedfe..0000000
--- a/60xx/libsensors_iio/software/build/android/shared.mk
+++ /dev/null
@@ -1,74 +0,0 @@
-SHELL=/bin/bash
-
-TARGET ?= android
-PRODUCT ?= beagleboard
-ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair
-KERNEL_ROOT ?= /Android/trunk/0xdroid/kernel
-MLSDK_ROOT ?= $(CURDIR)
-
-ifeq ($(VERBOSE),1)
- DUMP=1>/dev/stdout
-else
- DUMP=1>/dev/null
-endif
-
-include common.mk
-
-################################################################################
-## targets
-
-INV_ROOT = ../..
-LIB_FOLDERS = $(INV_ROOT)/core/mllite/build/$(TARGET)
-ifeq ($(BUILD_MPL),1)
- LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET)
-endif
-APP_FOLDERS = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET)
-APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET)
-APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET)
-
-INSTALL_DIR = $(CURDIR)
-
-################################################################################
-## macros
-
-define echo_in_colors
- echo -ne "\e[1;34m"$(1)"\e[0m"
-endef
-
-################################################################################
-## rules
-
-.PHONY : all mllite mpl clean
-
-all:
- for DIR in $(LIB_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
- for DIR in $(APP_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
-
-clean:
- for DIR in $(LIB_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
- for DIR in $(APP_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
-
-cleanall:
- for DIR in $(LIB_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
- for DIR in $(APP_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
-
-install:
- for DIR in $(LIB_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
- for DIR in $(APP_FOLDERS); do ( \
- cd $$DIR && $(MAKE) -f shared.mk $@ ); \
- done
-
diff --git a/60xx/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h
deleted file mode 100644
index c519691..0000000
--- a/60xx/libsensors_iio/software/core/driver/include/log.h
+++ /dev/null
@@ -1,364 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*
- * This file incorporates work covered by the following copyright and
- * permission notice:
- *
- * Copyright (C) 2005 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/*
- * C/C++ logging functions. See the logging documentation for API details.
- *
- * We'd like these to be available from C code (in case we import some from
- * somewhere), so this has a C interface.
- *
- * The output will be correct when the log file is shared between multiple
- * threads and/or multiple processes so long as the operating system
- * supports O_APPEND. These calls have mutex-protected data structures
- * and so are NOT reentrant. Do not use MPL_LOG in a signal handler.
- */
-#ifndef _LIBS_CUTILS_MPL_LOG_H
-#define _LIBS_CUTILS_MPL_LOG_H
-
-#include <stdlib.h>
-#include <stdarg.h>
-
-#ifdef ANDROID
-#ifdef NDK_BUILD
-#include "log_macros.h"
-#else
-#include <utils/Log.h> /* For the LOG macro */
-#endif
-#endif
-
-#ifdef __KERNEL__
-#include <linux/kernel.h>
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
- * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
- * at the top of your source file) to change that behavior.
- */
-#ifndef MPL_LOG_NDEBUG
-#ifdef NDEBUG
-#define MPL_LOG_NDEBUG 1
-#else
-#define MPL_LOG_NDEBUG 0
-#endif
-#endif
-
-#ifdef __KERNEL__
-#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
-#define MPL_LOG_DEFAULT KERN_DEFAULT
-#define MPL_LOG_VERBOSE KERN_CONT
-#define MPL_LOG_DEBUG KERN_NOTICE
-#define MPL_LOG_INFO KERN_INFO
-#define MPL_LOG_WARN KERN_WARNING
-#define MPL_LOG_ERROR KERN_ERR
-#define MPL_LOG_SILENT MPL_LOG_VERBOSE
-
-#else
- /* Based off the log priorities in android
- /system/core/include/android/log.h */
-#define MPL_LOG_UNKNOWN (0)
-#define MPL_LOG_DEFAULT (1)
-#define MPL_LOG_VERBOSE (2)
-#define MPL_LOG_DEBUG (3)
-#define MPL_LOG_INFO (4)
-#define MPL_LOG_WARN (5)
-#define MPL_LOG_ERROR (6)
-#define MPL_LOG_SILENT (8)
-#endif
-
-
-/*
- * This is the local tag used for the following simplified
- * logging macros. You can change this preprocessor definition
- * before using the other macros to change the tag.
- */
-#ifndef MPL_LOG_TAG
-#ifdef __KERNEL__
-#define MPL_LOG_TAG
-#else
-#define MPL_LOG_TAG NULL
-#endif
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGV
-#if MPL_LOG_NDEBUG
-#ifdef _WIN32
-#define MPL_LOGV(fmt, ...) \
- do { \
- __pragma (warning(suppress : 4127 )) \
- if (0) \
- MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
- __pragma (warning(suppress : 4127 )) \
- } while (0)
-#else
-#define MPL_LOGV(fmt, ...) \
- do { \
- if (0) \
- MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
- } while (0)
-#endif
-
-#else
-#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef CONDITION
-#define CONDITION(cond) ((cond) != 0)
-#endif
-
-#ifndef MPL_LOGV_IF
-#if MPL_LOG_NDEBUG
-#define MPL_LOGV_IF(cond, fmt, ...) \
- do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
-#else
-#define MPL_LOGV_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-#endif
-
-/*
- * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGD
-#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-
-#ifndef MPL_LOGD_IF
-#define MPL_LOGD_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/*
- * Simplified macro to send an info log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGI
-#ifdef __KERNEL__
-#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef MPL_LOGI_IF
-#define MPL_LOGI_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/*
- * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGW
-#ifdef __KERNEL__
-#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef MPL_LOGW_IF
-#define MPL_LOGW_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/*
- * Simplified macro to send an error log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGE
-#ifdef __KERNEL__
-#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef MPL_LOGE_IF
-#define MPL_LOGE_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Log a fatal error. If the given condition fails, this stops program
- * execution like a normal assertion, but also generating the given message.
- * It is NOT stripped from release builds. Note that the condition test
- * is -inverted- from the normal assert() semantics.
- */
-#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \
- fmt, ##__VA_ARGS__)) \
- : (void)0)
-
-#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
- (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
-
-/*
- * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
- * are stripped out of release builds.
- */
-#if MPL_LOG_NDEBUG
-#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
- do { \
- if (0) \
- MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
- } while (0)
-#define MPL_LOG_FATAL(fmt, ...) \
- do { \
- if (0) \
- MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \
- } while (0)
-#else
-#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
- MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
-#define MPL_LOG_FATAL(fmt, ...) \
- MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
-#endif
-
-/*
- * Assertion that generates a log message when the assertion fails.
- * Stripped out of release builds. Uses the current MPL_LOG_TAG.
- */
-#define MPL_LOG_ASSERT(cond, fmt, ...) \
- MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Basic log message macro.
- *
- * Example:
- * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
- *
- * The second argument may be NULL or "" to indicate the "global" tag.
- */
-#ifndef MPL_LOG
-#define MPL_LOG(priority, tag, fmt, ...) \
- MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
-#endif
-
-/*
- * Log macro that allows you to specify a number for the priority.
- */
-#ifndef MPL_LOG_PRI
-#ifdef ANDROID
-#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- ALOG(priority, tag, fmt, ##__VA_ARGS__)
-#elif defined __KERNEL__
-#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-/*
- * Log macro that allows you to pass in a varargs ("args" is a va_list).
- */
-#ifndef MPL_LOG_PRI_VA
-#ifdef ANDROID
-#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
- android_vprintLog(priority, NULL, tag, fmt, args)
-#elif defined __KERNEL__
-/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
-#else
-#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
- _MLPrintVaLog(priority, NULL, tag, fmt, args)
-#endif
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * ===========================================================================
- *
- * The stuff in the rest of this file should not be used directly.
- */
-
-#ifndef ANDROID
-int _MLPrintLog(int priority, const char *tag, const char *fmt, ...);
-int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
-/* Final implementation of actual writing to a character device */
-int _MLWriteLog(const char *buf, int buflen);
-#endif
-
-static inline void __print_result_location(int result,
- const char *file,
- const char *func, int line)
-{
- MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
-}
-
-#ifdef _WIN32
-/* The pragma removes warning about expression being constant */
-#define LOG_RESULT_LOCATION(condition) \
- do { \
- __print_result_location((int)(condition), __FILE__, \
- __func__, __LINE__); \
- __pragma (warning(suppress : 4127 )) \
- } while (0)
-#else
-#define LOG_RESULT_LOCATION(condition) \
- do { \
- __print_result_location((int)(condition), __FILE__, \
- __func__, __LINE__); \
- } while (0)
-#endif
-
-
-#define INV_ERROR_CHECK(r_1329) \
- if (r_1329) { \
- LOG_RESULT_LOCATION(r_1329); \
- return r_1329; \
- }
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/60xx/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
deleted file mode 100644
index 9725199..0000000
--- a/60xx/libsensors_iio/software/core/driver/include/mlinclude.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-#ifndef INV_INCLUDE_H__
-#define INV_INCLUDE_H__
-
-#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere
-
-#ifdef COVERAGE
-#include "utestCommon.h"
-#endif
-#ifdef PROFILE
-#include "profile.h"
-#endif
-
-#ifdef WIN32
-#ifdef COVERAGE
-
-extern int functionEnterLog(const char *file, const char *func);
-extern int functionExitLog(const char *file, const char *func);
-
-#undef INVENSENSE_FUNC_START
-#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \
- int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__)
-#endif // COVERAGE
-#endif // WIN32
-
-#ifdef PROFILE
-#undef INVENSENSE_FUNC_START
-#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__)
-#define return if ( profileExit(__FILE__, __FUNCTION__) ) return
-#endif // PROFILE
-
-// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return
-
-#endif //INV_INCLUDE_H__
diff --git a/60xx/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h
deleted file mode 100644
index 37194d6..0000000
--- a/60xx/libsensors_iio/software/core/driver/include/mlmath.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-/*******************************************************************************
- *
- * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef _ML_MATH_H_
-#define _ML_MATH_H_
-
-#ifndef MLMATH
-// This define makes Microsoft pickup things like M_PI
-#define _USE_MATH_DEFINES
-#include <math.h>
-
-#ifdef WIN32
-// Microsoft doesn't follow standards
-#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5))))
-#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
-#endif
-
-#else // MLMATH
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-/* MPL needs below functions */
-double ml_asin(double);
-double ml_atan(double);
-double ml_atan2(double, double);
-double ml_log(double);
-double ml_sqrt(double);
-double ml_ceil(double);
-double ml_floor(double);
-double ml_cos(double);
-double ml_sin(double);
-double ml_acos(double);
-#ifdef __cplusplus
-} // extern "C"
-#endif
-
-/*
- * We rename functions here to provide the hook for other
- * customized math functions.
- */
-#define sqrt(x) ml_sqrt(x)
-#define log(x) ml_log(x)
-#define asin(x) ml_asin(x)
-#define atan(x) ml_atan(x)
-#define atan2(x,y) ml_atan2(x,y)
-#define ceil(x) ml_ceil(x)
-#define floor(x) ml_floor(x)
-#define fabs(x) (((x)<0)?-(x):(x))
-#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5))))
-#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
-#define cos(x) ml_cos(x)
-#define sin(x) ml_sin(x)
-#define acos(x) ml_acos(x)
-
-#define pow(x,y) ml_pow(x,y)
-
-#ifdef LINUX
-/* stubs for float version of math functions */
-#define cosf(x) ml_cos(x)
-#define sinf(x) ml_sin(x)
-#define atan2f(x,y) ml_atan2(x,y)
-#define sqrtf(x) ml_sqrt(x)
-#endif
-
-
-
-#endif // MLMATH
-
-#ifndef M_PI
-#define M_PI 3.14159265358979
-#endif
-
-#ifndef ABS
-#define ABS(x) (((x)>=0)?(x):-(x))
-#endif
-
-#ifndef MIN
-#define MIN(x,y) (((x)<(y))?(x):(y))
-#endif
-
-#ifndef MAX
-#define MAX(x,y) (((x)>(y))?(x):(y))
-#endif
-
-/*---------------------------*/
-#endif /* !_ML_MATH_H_ */
diff --git a/60xx/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h
deleted file mode 100644
index 12f2901..0000000
--- a/60xx/libsensors_iio/software/core/driver/include/mlsl.h
+++ /dev/null
@@ -1,283 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-#ifndef __MLSL_H__
-#define __MLSL_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/**
- * @defgroup MLSL
- * @brief Motion Library - Serial Layer.
- * The Motion Library System Layer provides the Motion Library
- * with the communication interface to the hardware.
- *
- * The communication interface is assumed to support serial
- * transfers in burst of variable length up to
- * SERIAL_MAX_TRANSFER_SIZE.
- * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes.
- * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will
- * be subdivided in smaller transfers of length <=
- * SERIAL_MAX_TRANSFER_SIZE.
- * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to
- * overcome any host processor transfer size limitation down to
- * 1 B, the minimum.
- * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor
- * performance and efficiency while requiring higher resource usage
- * (mostly buffering). A smaller value will increase overhead and
- * decrease efficiency but allows to operate with more resource
- * constrained processor and master serial controllers.
- * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
- * mlsl.h header file and master serial controllers.
- * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
- * mlsl.h header file.
- *
- * @{
- * @file mlsl.h
- * @brief The Motion Library System Layer.
- *
- */
-
-/*
- * NOTE : to properly support Yamaha compass reads,
- * the max transfer size should be at least 9 B.
- * Length in bytes, typically a power of 2 >= 2
- */
-#define SERIAL_MAX_TRANSFER_SIZE 31
-
-#ifndef __KERNEL__
-/**
- * inv_serial_open() - used to open the serial port.
- * @port The COM port specification associated with the device in use.
- * @sl_handle a pointer to the file handle to the serial device to be open
- * for the communication.
- * This port is used to send and receive data to the device.
- *
- * This function is called by inv_serial_start().
- * Unlike previous MPL Software releases, explicitly calling
- * inv_serial_start() is mandatory to instantiate the communication
- * with the device.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_open(char const *port, void **sl_handle);
-
-/**
- * inv_serial_close() - used to close the serial port.
- * @sl_handle a file handle to the serial device used for the communication.
- *
- * This port is used to send and receive data to the device.
- *
- * This function is called by inv_serial_stop().
- * Unlike previous MPL Software releases, explicitly calling
- * inv_serial_stop() is mandatory to properly shut-down the
- * communication with the device.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_close(void *sl_handle);
-
-/**
- * inv_serial_reset() - used to reset any buffering the driver may be doing
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_reset(void *sl_handle);
-#endif
-
-/**
- * inv_serial_single_write() - used to write a single byte of data.
- * @sl_handle pointer to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @register_addr Register address to write.
- * @data Single byte of data to write.
- *
- * It is called by the MPL to write a single byte of data to the MPU.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_single_write(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned char register_addr,
- unsigned char data);
-
-/**
- * inv_serial_write() - used to write multiple bytes of data to registers.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @register_addr Register address to write.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_write(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short length,
- unsigned char const *data);
-
-/**
- * inv_serial_read() - used to read multiple bytes of data from registers.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @register_addr Register address to read.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-int inv_serial_read(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned char register_addr,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_read_mem() - used to read multiple bytes of data from the memory.
- * This should be sent by I2C or SPI.
- *
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @mem_addr The location in the memory to read from.
- * @length Length of burst data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-int inv_serial_read_mem(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short mem_addr,
- unsigned char bank_reg,
- unsigned char addr_reg,
- unsigned char mem_reg,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_write_mem() - used to write multiple bytes of data to the memory.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @mem_addr The location in the memory to write to.
- * @length Length of burst data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-int inv_serial_write_mem(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short mem_addr,
- unsigned char bank_reg,
- unsigned char addr_reg,
- unsigned char mem_reg,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-int inv_serial_read_fifo(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned char fifo_reg,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-int inv_serial_write_fifo(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned char fifo_reg,
- unsigned short length,
- unsigned char const *data);
-
-#ifndef __KERNEL__
-/**
- * inv_serial_read_cfg() - used to get the configuration data.
- * @cfg Pointer to the configuration data.
- * @len Length of the configuration data.
- *
- * Is called by the MPL to get the configuration data
- * used by the motion library.
- * This data would typically be saved in non-volatile memory.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
-
-/**
- * inv_serial_write_cfg() - used to save the configuration data.
- * @cfg Pointer to the configuration data.
- * @len Length of the configuration data.
- *
- * Is called by the MPL to save the configuration data used by the
- * motion library.
- * This data would typically be saved in non-volatile memory.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
-
-/**
- * inv_serial_read_cal() - used to get the calibration data.
- * @cfg Pointer to the calibration data.
- * @len Length of the calibration data.
- *
- * It is called by the MPL to get the calibration data used by the
- * motion library.
- * This data is typically be saved in non-volatile memory.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_read_cal(unsigned char *cal, unsigned int len);
-
-/**
- * inv_serial_write_cal() - used to save the calibration data.
- *
- * @cfg Pointer to the calibration data.
- * @len Length of the calibration data.
- *
- * It is called by the MPL to save the calibration data used by the
- * motion library.
- * This data is typically be saved in non-volatile memory.
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_write_cal(unsigned char *cal, unsigned int len);
-
-/**
- * inv_serial_get_cal_length() - Get the calibration length from the storage.
- * @len lenght to be returned
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-int inv_serial_get_cal_length(unsigned int *len);
-#endif
-#ifdef __cplusplus
-}
-#endif
-/**
- * @}
- */
-#endif /* __MLSL_H__ */
diff --git a/60xx/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h
deleted file mode 100644
index 09eccce..0000000
--- a/60xx/libsensors_iio/software/core/driver/include/mltypes.h
+++ /dev/null
@@ -1,235 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/**
- * @defgroup MLERROR
- * @brief Motion Library - Error definitions.
- * Definition of the error codes used within the MPL and
- * returned to the user.
- * Every function tries to return a meaningful error code basing
- * on the occuring error condition. The error code is numeric.
- *
- * The available error codes and their associated values are:
- * - (0) INV_SUCCESS
- * - (32) INV_ERROR
- * - (22 / EINVAL) INV_ERROR_INVALID_PARAMETER
- * - (1 / EPERM) INV_ERROR_FEATURE_NOT_ENABLED
- * - (36) INV_ERROR_FEATURE_NOT_IMPLEMENTED
- * - (38) INV_ERROR_DMP_NOT_STARTED
- * - (39) INV_ERROR_DMP_STARTED
- * - (40) INV_ERROR_NOT_OPENED
- * - (41) INV_ERROR_OPENED
- * - (19 / ENODEV) INV_ERROR_INVALID_MODULE
- * - (12 / ENOMEM) INV_ERROR_MEMORY_EXAUSTED
- * - (44) INV_ERROR_DIVIDE_BY_ZERO
- * - (45) INV_ERROR_ASSERTION_FAILURE
- * - (46) INV_ERROR_FILE_OPEN
- * - (47) INV_ERROR_FILE_READ
- * - (48) INV_ERROR_FILE_WRITE
- * - (49) INV_ERROR_INVALID_CONFIGURATION
- * - (52) INV_ERROR_SERIAL_CLOSED
- * - (53) INV_ERROR_SERIAL_OPEN_ERROR
- * - (54) INV_ERROR_SERIAL_READ
- * - (55) INV_ERROR_SERIAL_WRITE
- * - (56) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
- * - (57) INV_ERROR_SM_TRANSITION
- * - (58) INV_ERROR_SM_IMPROPER_STATE
- * - (62) INV_ERROR_FIFO_OVERFLOW
- * - (63) INV_ERROR_FIFO_FOOTER
- * - (64) INV_ERROR_FIFO_READ_COUNT
- * - (65) INV_ERROR_FIFO_READ_DATA
- * - (72) INV_ERROR_MEMORY_SET
- * - (82) INV_ERROR_LOG_MEMORY_ERROR
- * - (83) INV_ERROR_LOG_OUTPUT_ERROR
- * - (92) INV_ERROR_OS_BAD_PTR
- * - (93) INV_ERROR_OS_BAD_HANDLE
- * - (94) INV_ERROR_OS_CREATE_FAILED
- * - (95) INV_ERROR_OS_LOCK_FAILED
- * - (102) INV_ERROR_COMPASS_DATA_OVERFLOW
- * - (103) INV_ERROR_COMPASS_DATA_UNDERFLOW
- * - (104) INV_ERROR_COMPASS_DATA_NOT_READY
- * - (105) INV_ERROR_COMPASS_DATA_ERROR
- * - (107) INV_ERROR_CALIBRATION_LOAD
- * - (108) INV_ERROR_CALIBRATION_STORE
- * - (109) INV_ERROR_CALIBRATION_LEN
- * - (110) INV_ERROR_CALIBRATION_CHECKSUM
- * - (111) INV_ERROR_ACCEL_DATA_OVERFLOW
- * - (112) INV_ERROR_ACCEL_DATA_UNDERFLOW
- * - (113) INV_ERROR_ACCEL_DATA_NOT_READY
- * - (114) INV_ERROR_ACCEL_DATA_ERROR
- *
- * The available warning codes and their associated values are:
- * - (115) INV_WARNING_MOTION_RACE
- * - (116) INV_WARNING_QUAT_TRASHED
- *
- * @{
- * @file mltypes.h
- * @}
- */
-
-#ifndef MLTYPES_H
-#define MLTYPES_H
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#include <asm-generic/errno-base.h>
-#else
-#include "stdint_invensense.h"
-#include <errno.h>
-#endif
-#include <limits.h>
-
-#ifndef REMOVE_INV_ERROR_T
-/*---------------------------
- * ML Types
- *--------------------------*/
-
-/**
- * @struct inv_error_t mltypes.h "mltypes"
- * @brief The MPL Error Code return type.
- *
- * @code
- * typedef unsigned char inv_error_t;
- * @endcode
- */
-//typedef unsigned char inv_error_t;
-typedef int inv_error_t;
-#endif
-
-typedef long long inv_time_t;
-
-#if !defined __GNUC__ && !defined __KERNEL__
-typedef int8_t __s8;
-typedef int16_t __s16;
-typedef int32_t __s32;
-typedef int32_t __s64;
-
-typedef uint8_t __u8;
-typedef uint16_t __u16;
-typedef uint32_t __u32;
-typedef uint64_t __u64;
-#elif !defined __KERNEL__
-#include <sys/types.h>
-#endif
-
-#ifndef __cplusplus
-#ifndef __KERNEL__
-typedef int_fast8_t bool;
-
-#ifndef false
-#define false 0
-#endif
-
-#ifndef true
-#define true 1
-#endif
-
-#endif
-#endif
-
-/*---------------------------
- * ML Defines
- *--------------------------*/
-
-#ifndef NULL
-#define NULL 0
-#endif
-
-#ifndef __KERNEL__
-#ifndef ARRAY_SIZE
-/* Dimension of an array */
-#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0]))
-#endif
-#endif
-/* - ML Errors. - */
-#define ERROR_NAME(x) (#x)
-#define ERROR_CHECK_FIRST(first, x) \
- { if (INV_SUCCESS == first) first = x; }
-
-#define INV_SUCCESS (0)
-/* Generic Error code. Proprietary Error Codes only */
-#define INV_ERROR_BASE (0x20)
-#define INV_ERROR (INV_ERROR_BASE)
-
-/* Compatibility and other generic error codes */
-#define INV_ERROR_INVALID_PARAMETER (EINVAL)
-#define INV_ERROR_FEATURE_NOT_ENABLED (EPERM)
-#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (INV_ERROR_BASE + 4)
-#define INV_ERROR_DMP_NOT_STARTED (INV_ERROR_BASE + 6)
-#define INV_ERROR_DMP_STARTED (INV_ERROR_BASE + 7)
-#define INV_ERROR_NOT_OPENED (INV_ERROR_BASE + 8)
-#define INV_ERROR_OPENED (INV_ERROR_BASE + 9)
-#define INV_ERROR_INVALID_MODULE (ENODEV)
-#define INV_ERROR_MEMORY_EXAUSTED (ENOMEM)
-#define INV_ERROR_DIVIDE_BY_ZERO (INV_ERROR_BASE + 12)
-#define INV_ERROR_ASSERTION_FAILURE (INV_ERROR_BASE + 13)
-#define INV_ERROR_FILE_OPEN (INV_ERROR_BASE + 14)
-#define INV_ERROR_FILE_READ (INV_ERROR_BASE + 15)
-#define INV_ERROR_FILE_WRITE (INV_ERROR_BASE + 16)
-#define INV_ERROR_INVALID_CONFIGURATION (INV_ERROR_BASE + 17)
-#define INV_ERROR_NOT_AUTHORIZED (INV_ERROR_BASE + 18)
-
-/* Serial Communication */
-#define INV_ERROR_SERIAL_CLOSED (INV_ERROR_BASE + 20)
-#define INV_ERROR_SERIAL_OPEN_ERROR (INV_ERROR_BASE + 21)
-#define INV_ERROR_SERIAL_READ (INV_ERROR_BASE + 22)
-#define INV_ERROR_SERIAL_WRITE (INV_ERROR_BASE + 23)
-#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (INV_ERROR_BASE + 24)
-
-/* SM = State Machine */
-#define INV_ERROR_SM_TRANSITION (INV_ERROR_BASE + 25)
-#define INV_ERROR_SM_IMPROPER_STATE (INV_ERROR_BASE + 26)
-
-/* Fifo */
-#define INV_ERROR_FIFO_OVERFLOW (INV_ERROR_BASE + 30)
-#define INV_ERROR_FIFO_FOOTER (INV_ERROR_BASE + 31)
-#define INV_ERROR_FIFO_READ_COUNT (INV_ERROR_BASE + 32)
-#define INV_ERROR_FIFO_READ_DATA (INV_ERROR_BASE + 33)
-
-/* Memory & Registers, Set & Get */
-#define INV_ERROR_MEMORY_SET (INV_ERROR_BASE + 40)
-
-#define INV_ERROR_LOG_MEMORY_ERROR (INV_ERROR_BASE + 50)
-#define INV_ERROR_LOG_OUTPUT_ERROR (INV_ERROR_BASE + 51)
-
-/* OS interface errors */
-#define INV_ERROR_OS_BAD_PTR (INV_ERROR_BASE + 60)
-#define INV_ERROR_OS_BAD_HANDLE (INV_ERROR_BASE + 61)
-#define INV_ERROR_OS_CREATE_FAILED (INV_ERROR_BASE + 62)
-#define INV_ERROR_OS_LOCK_FAILED (INV_ERROR_BASE + 63)
-
-/* Compass errors */
-#define INV_ERROR_COMPASS_DATA_OVERFLOW (INV_ERROR_BASE + 70)
-#define INV_ERROR_COMPASS_DATA_UNDERFLOW (INV_ERROR_BASE + 71)
-#define INV_ERROR_COMPASS_DATA_NOT_READY (INV_ERROR_BASE + 72)
-#define INV_ERROR_COMPASS_DATA_ERROR (INV_ERROR_BASE + 73)
-
-/* Load/Store calibration */
-#define INV_ERROR_CALIBRATION_LOAD (INV_ERROR_BASE + 75)
-#define INV_ERROR_CALIBRATION_STORE (INV_ERROR_BASE + 76)
-#define INV_ERROR_CALIBRATION_LEN (INV_ERROR_BASE + 77)
-#define INV_ERROR_CALIBRATION_CHECKSUM (INV_ERROR_BASE + 78)
-
-/* Accel errors */
-#define INV_ERROR_ACCEL_DATA_OVERFLOW (INV_ERROR_BASE + 79)
-#define INV_ERROR_ACCEL_DATA_UNDERFLOW (INV_ERROR_BASE + 80)
-#define INV_ERROR_ACCEL_DATA_NOT_READY (INV_ERROR_BASE + 81)
-#define INV_ERROR_ACCEL_DATA_ERROR (INV_ERROR_BASE + 82)
-
-/* No Motion Warning States */
-#define INV_WARNING_MOTION_RACE (INV_ERROR_BASE + 83)
-#define INV_WARNING_QUAT_TRASHED (INV_ERROR_BASE + 84)
-#define INV_WARNING_GYRO_MAG (INV_ERROR_BASE + 85)
-
-#define INV_WARNING_SEMAPHORE_TIMEOUT (INV_ERROR_BASE + 86)
-
-
-/* For Linux coding compliance */
-#ifndef __KERNEL__
-#define EXPORT_SYMBOL(x)
-#endif
-
-#endif /* MLTYPES_H */
diff --git a/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
deleted file mode 100644
index b8c2511..0000000
--- a/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-#ifndef STDINT_INVENSENSE_H
-#define STDINT_INVENSENSE_H
-
-#ifndef WIN32
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#else
-#include <stdint.h>
-#endif
-
-#else
-
-#include <windows.h>
-
-typedef signed char int8_t;
-typedef short int16_t;
-typedef long int32_t;
-typedef long long int64_t;
-
-typedef unsigned char uint8_t;
-typedef unsigned short uint16_t;
-typedef unsigned long uint32_t;
-typedef unsigned long long uint64_t;
-
-typedef int int_fast8_t;
-typedef int int_fast16_t;
-typedef long int_fast32_t;
-
-typedef unsigned int uint_fast8_t;
-typedef unsigned int uint_fast16_t;
-typedef unsigned long uint_fast32_t;
-
-#endif
-
-#endif // STDINT_INVENSENSE_H
diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
deleted file mode 100755
index 9bdd5bc..0000000
--- a/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so
+++ /dev/null
Binary files differ
diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
deleted file mode 100644
index 1418450..0000000
--- a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk
+++ /dev/null
@@ -1,87 +0,0 @@
-MLLITE_LIB_NAME = mllite
-LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
-
-MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
-
-CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
-COMP ?= $(CROSS)gcc
-LINK ?= $(CROSS)gcc
-
-OBJFOLDER = $(CURDIR)/obj
-
-INV_ROOT = ../../../../..
-MLLITE_DIR = $(INV_ROOT)/software/core/mllite
-
-include $(INV_ROOT)/software/build/android/common.mk
-
-CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += $(ANDROID_COMPILE)
-CFLAGS += -Wall
-CFLAGS += -fpic
-CFLAGS += -nostdlib
-CFLAGS += -DNDEBUG
-CFLAGS += -D_REENTRANT
-CFLAGS += -DLINUX
-CFLAGS += -DANDROID
-CFLAGS += -mthumb-interwork
-CFLAGS += -fno-exceptions
-CFLAGS += -ffunction-sections
-CFLAGS += -funwind-tables
-CFLAGS += -fstack-protector
-CFLAGS += -fno-short-enums
-CFLAGS += -fmessage-length=0
-CFLAGS += -I$(MLLITE_DIR)
-CFLAGS += -I$(INV_ROOT)/simple_apps/common
-CFLAGS += $(INV_INCLUDES)
-CFLAGS += $(INV_DEFINES)
-
-LLINK = -lc
-LLINK += -lm
-LLINK += -lutils
-LLINK += -lcutils
-LLINK += -lgcc
-LLINK += -ldl
-
-LFLAGS += $(CMDLINE_LFLAGS)
-LFLAGS += -shared
-LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -Wl,-shared,-Bsymbolic
-LFLAGS += $(ANDROID_LINK)
-LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
-
-####################################################################################################
-## sources
-
-#INV_SOURCES provided by Makefile.filelist
-include ../filelist.mk
-
-INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
-INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
-
-####################################################################################################
-## rules
-
-.PHONY: all mllite clean cleanall makefiles
-
-all: mllite
-
-mllite: $(LIBRARY) $(MK_NAME)
-
-$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
- @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
- $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
-
-$(OBJFOLDER) :
- @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
- mkdir obj
-
-$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME)
- @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
- $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
-
-clean :
- rm -fR $(OBJFOLDER)
-
-cleanall :
- rm -fR $(LIBRARY) $(OBJFOLDER)
-
diff --git a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
deleted file mode 100644
index 011120c..0000000
--- a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-#### filelist.mk for mllite ####
-
-# headers only
-HEADERS := $(MLLITE_DIR)/invensense.h
-
-# headers for sources
-HEADERS += $(MLLITE_DIR)/data_builder.h
-HEADERS += $(MLLITE_DIR)/hal_outputs.h
-HEADERS += $(MLLITE_DIR)/message_layer.h
-HEADERS += $(MLLITE_DIR)/ml_math_func.h
-HEADERS += $(MLLITE_DIR)/mpl.h
-HEADERS += $(MLLITE_DIR)/results_holder.h
-HEADERS += $(MLLITE_DIR)/start_manager.h
-HEADERS += $(MLLITE_DIR)/storage_manager.h
-
-# headers (linux specific)
-HEADERS += $(MLLITE_DIR)/linux/mlos.h
-HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h
-HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h
-HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h
-
-# sources
-SOURCES := $(MLLITE_DIR)/data_builder.c
-SOURCES += $(MLLITE_DIR)/hal_outputs.c
-SOURCES += $(MLLITE_DIR)/message_layer.c
-SOURCES += $(MLLITE_DIR)/ml_math_func.c
-SOURCES += $(MLLITE_DIR)/mpl.c
-SOURCES += $(MLLITE_DIR)/results_holder.c
-SOURCES += $(MLLITE_DIR)/start_manager.c
-SOURCES += $(MLLITE_DIR)/storage_manager.c
-
-# sources (linux specific)
-SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c
-SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c
-SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c
-SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c
-
-
-INV_SOURCES += $(SOURCES)
-
-VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux
-
diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c
deleted file mode 100644
index 0aa418d..0000000
--- a/60xx/libsensors_iio/software/core/mllite/data_builder.c
+++ /dev/null
@@ -1,1228 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/**
- * @defgroup Data_Builder data_builder
- * @brief Motion Library - Data Builder
- * Constructs and Creates the data for MPL
- *
- * @{
- * @file data_builder.c
- * @brief Data Builder.
- */
-
-#undef MPL_LOG_NDEBUG
-#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
-
-#include <string.h>
-
-#include "ml_math_func.h"
-#include "data_builder.h"
-#include "mlmath.h"
-#include "storage_manager.h"
-#include "message_layer.h"
-#include "results_holder.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL"
-
-typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data);
-
-struct process_t {
- inv_process_cb_func func;
- int priority;
- int data_required;
-};
-
-struct inv_db_save_t {
- /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */
- long compass_bias[3];
- /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */
- long gyro_bias[3];
- /** Temperature when *gyro_bias was stored. */
- long gyro_temp;
- /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */
- long accel_bias[3];
- /** Temperature when accel bias was stored. */
- long accel_temp;
- long gyro_temp_slope[3];
- /** Sensor Accuracy */
- int gyro_accuracy;
- int accel_accuracy;
- int compass_accuracy;
-};
-
-struct inv_data_builder_t {
- int num_cb;
- struct process_t process[INV_MAX_DATA_CB];
- struct inv_db_save_t save;
- int compass_disturbance;
- int mode;
-#ifdef INV_PLAYBACK_DBG
- int debug_mode;
- int last_mode;
- FILE *file;
-#endif
-};
-
-void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias);
-static void inv_set_contiguous(void);
-
-static struct inv_data_builder_t inv_data_builder;
-static struct inv_sensor_cal_t sensors;
-
-/** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53395
-
-#ifdef INV_PLAYBACK_DBG
-
-/** Turn on data logging to allow playback of same scenario at a later time.
-* @param[in] file File to write to, must be open.
-*/
-void inv_turn_on_data_logging(FILE *file)
-{
- MPL_LOGV("input data logging started\n");
- inv_data_builder.file = file;
- inv_data_builder.debug_mode = RD_RECORD;
-}
-
-/** Turn off data logging to allow playback of same scenario at a later time.
-* File passed to inv_turn_on_data_logging() must be closed after calling this.
-*/
-void inv_turn_off_data_logging()
-{
- MPL_LOGV("input data logging stopped\n");
- inv_data_builder.debug_mode = RD_NO_DEBUG;
- inv_data_builder.file = NULL;
-}
-#endif
-
-/** This function receives the data that was stored in non-volatile memory between power off */
-static inv_error_t inv_db_load_func(const unsigned char *data)
-{
- memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
- // copy in the saved accuracy in the actual sensors accuracy
- sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
- sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
- sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
- // TODO
- if (sensors.compass.accuracy == 3) {
- inv_set_compass_bias_found(1);
- }
- return INV_SUCCESS;
-}
-
-/** This function returns the data to be stored in non-volatile memory between power off */
-static inv_error_t inv_db_save_func(unsigned char *data)
-{
- memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save));
- return INV_SUCCESS;
-}
-
-/** Initialize the data builder
-*/
-inv_error_t inv_init_data_builder(void)
-{
- /* TODO: Hardcode temperature scale/offset here. */
- memset(&inv_data_builder, 0, sizeof(inv_data_builder));
- memset(&sensors, 0, sizeof(sensors));
- return inv_register_load_store(inv_db_load_func, inv_db_save_func,
- sizeof(inv_data_builder.save),
- INV_DB_SAVE_KEY);
-}
-
-/** Gyro sensitivity.
-* @return A scale factor to convert device units to degrees per second scaled by 2^16
-* such that degrees_per_second = device_units * sensitivity / 2^30. Typically
-* it works out to be the maximum rate * 2^15.
-*/
-long inv_get_gyro_sensitivity()
-{
- return sensors.gyro.sensitivity;
-}
-
-/** Accel sensitivity.
-* @return A scale factor to convert device units to g's scaled by 2^16
-* such that g_s = device_units * sensitivity / 2^30. Typically
-* it works out to be the maximum accel value in g's * 2^15.
-*/
-long inv_get_accel_sensitivity(void)
-{
- return sensors.accel.sensitivity;
-}
-
-/** Compass sensitivity.
-* @return A scale factor to convert device units to micro Tesla scaled by 2^16
-* such that uT = device_units * sensitivity / 2^30. Typically
-* it works out to be the maximum uT * 2^15.
-*/
-long inv_get_compass_sensitivity(void)
-{
- return sensors.compass.sensitivity;
-}
-
-/** Sets orientation and sensitivity field for a sensor.
-* @param[out] sensor Structure to apply settings to
-* @param[in] orientation Orientation description of how part is mounted.
-* @param[in] sensitivity A Scale factor to convert from hardware units to
-* standard units (dps, uT, g).
-*/
-void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
- int orientation, long sensitivity)
-{
- sensor->sensitivity = sensitivity;
- sensor->orientation = orientation;
-}
-
-/** Sets the Orientation and Sensitivity of the gyro data.
-* @param[in] orientation A scalar defining the transformation from chip mounting
-* to the body frame. The function inv_orientation_matrix_to_scalar()
-* can convert the transformation matrix to this scalar and describes the
-* scalar in further detail.
-* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16
-* such that degrees_per_second = device_units * sensitivity / 2^30. Typically
-* it works out to be the maximum rate * 2^15.
-*/
-void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_G_ORIENT;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
- fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
- }
-#endif
- set_sensor_orientation_and_scale(&sensors.gyro, orientation,
- sensitivity);
-}
-
-/** Set Gyro Sample rate in micro seconds.
-* @param[in] sample_rate_us Set Gyro Sample rate in us
-*/
-void inv_set_gyro_sample_rate(long sample_rate_us)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
- }
-#endif
- sensors.gyro.sample_rate_us = sample_rate_us;
- sensors.gyro.sample_rate_ms = sample_rate_us / 1000;
- if (sensors.gyro.bandwidth == 0) {
- sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us);
- }
-}
-
-/** Set Accel Sample rate in micro seconds.
-* @param[in] sample_rate_us Set Accel Sample rate in us
-*/
-void inv_set_accel_sample_rate(long sample_rate_us)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
- }
-#endif
- sensors.accel.sample_rate_us = sample_rate_us;
- sensors.accel.sample_rate_ms = sample_rate_us / 1000;
- if (sensors.accel.bandwidth == 0) {
- sensors.accel.bandwidth = (int)(1000000L / sample_rate_us);
- }
-}
-
-/** Pick the smallest non-negative number. Priority to td1 on equal
-* If both are negative, return the largest.
-*/
-static int inv_pick_best_time_difference(long td1, long td2)
-{
- if (td1 >= 0) {
- if (td2 >= 0) {
- if (td1 <= td2) {
- // td1
- return 0;
- } else {
- // td2
- return 1;
- }
- } else {
- // td1
- return 0;
- }
- } else if (td2 >= 0) {
- // td2
- return 1;
- } else {
- // Both are negative
- if (td1 >= td2) {
- // td1
- return 0;
- } else {
- // td2
- return 1;
- }
- }
-}
-
-/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data.
-*/
-static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts)
-{
- int status = 0;
- switch (sensor_number) {
- case 0: // Quat
- *ts = sensors.quat.timestamp;
- if (inv_data_builder.mode & INV_QUAT_NEW)
- if (sensors.quat.timestamp_prev != sensors.quat.timestamp)
- status = 1;
- return status;
- case 1: // Gyro
- *ts = sensors.gyro.timestamp;
- if (inv_data_builder.mode & INV_GYRO_NEW)
- if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp)
- status = 1;
- return status;
- case 2: // Accel
- *ts = sensors.accel.timestamp;
- if (inv_data_builder.mode & INV_ACCEL_NEW)
- if (sensors.accel.timestamp_prev != sensors.accel.timestamp)
- status = 1;
- return status;
- case 3: // Compass
- *ts = sensors.compass.timestamp;
- if (inv_data_builder.mode & INV_MAG_NEW)
- if (sensors.compass.timestamp_prev != sensors.compass.timestamp)
- status = 1;
- return status;
- default:
- *ts = 0;
- return 0;
- }
- return 0;
-}
-
-/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination.
-* It does this by finding a raw sensor that has the closest sample rate that is at least as
-* often desired. It also returns if that raw sensor has a new piece of data.
-* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel
-* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise.
-*/
-int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts)
-{
- long td[2];
- int idx;
-
- if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) {
- // Sensor number is 0 (Quat)
- return inv_raw_sensor_timestamp(0, ts);
- } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
- return 0; // Accel must be on or 6-axis quat must be on
- }
-
- // At this point, we know accel is on. Check if 3-axis quat is on
- td[0] = sample_rate_us - sensors.quat.sample_rate_us;
- td[1] = sample_rate_us - sensors.accel.sample_rate_us;
- if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) {
- idx = inv_pick_best_time_difference(td[0], td[1]);
- idx *= 2;
- // 0 = quat, 3=accel
- return inv_raw_sensor_timestamp(idx, ts);
- }
-
- // No Quaternion data from outside, Gyro must be on
- if ((sensors.gyro.status & INV_SENSOR_ON) == 0) {
- return 0; // Gyro must be on
- }
-
- td[0] = sample_rate_us - sensors.gyro.sample_rate_us;
- idx = inv_pick_best_time_difference(td[0], td[1]);
- idx *= 2; // 0=gyro 2=accel
- idx++;
- // 1 = gyro, 3=accel
- return inv_raw_sensor_timestamp(idx, ts);
-}
-
-/** Set Compass Sample rate in micro seconds.
-* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds.
-*/
-void inv_set_compass_sample_rate(long sample_rate_us)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
- }
-#endif
- sensors.compass.sample_rate_us = sample_rate_us;
- sensors.compass.sample_rate_ms = sample_rate_us / 1000;
- if (sensors.compass.bandwidth == 0) {
- sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
- }
-}
-
-void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
-{
- *sample_rate_ms = sensors.gyro.sample_rate_ms;
-}
-
-void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
-{
- *sample_rate_ms = sensors.accel.sample_rate_ms;
-}
-
-void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
-{
- *sample_rate_ms = sensors.compass.sample_rate_ms;
-}
-
-/** Set Quat Sample rate in micro seconds.
-* @param[in] sample_rate_us Set Quat Sample rate in us
-*/
-void inv_set_quat_sample_rate(long sample_rate_us)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file);
- }
-#endif
- sensors.quat.sample_rate_us = sample_rate_us;
- sensors.quat.sample_rate_ms = sample_rate_us / 1000;
-}
-
-/** Set Gyro Bandwidth in Hz
-* @param[in] bandwidth_hz Gyro bandwidth in Hz
-*/
-void inv_set_gyro_bandwidth(int bandwidth_hz)
-{
- sensors.gyro.bandwidth = bandwidth_hz;
-}
-
-/** Set Accel Bandwidth in Hz
-* @param[in] bandwidth_hz Gyro bandwidth in Hz
-*/
-void inv_set_accel_bandwidth(int bandwidth_hz)
-{
- sensors.accel.bandwidth = bandwidth_hz;
-}
-
-/** Set Compass Bandwidth in Hz
-* @param[in] bandwidth_hz Gyro bandwidth in Hz
-*/
-void inv_set_compass_bandwidth(int bandwidth_hz)
-{
- sensors.compass.bandwidth = bandwidth_hz;
-}
-
-/** Helper function stating whether the compass is on or off.
- * @return TRUE if compass if on, 0 if compass if off
-*/
-int inv_get_compass_on()
-{
- return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON;
-}
-
-/** Helper function stating whether the gyro is on or off.
- * @return TRUE if gyro if on, 0 if gyro if off
-*/
-int inv_get_gyro_on()
-{
- return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON;
-}
-
-/** Helper function stating whether the acceleromter is on or off.
- * @return TRUE if accel if on, 0 if accel if off
-*/
-int inv_get_accel_on()
-{
- return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON;
-}
-
-/** Get last timestamp across all 3 sensors that are on.
-* This find out which timestamp has the largest value for sensors that are on.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_time_t inv_get_last_timestamp()
-{
- inv_time_t timestamp = 0;
- if (sensors.accel.status & INV_SENSOR_ON) {
- timestamp = sensors.accel.timestamp;
- }
- if (sensors.gyro.status & INV_SENSOR_ON) {
- if (timestamp < sensors.gyro.timestamp) {
- timestamp = sensors.gyro.timestamp;
- }
- }
- if (sensors.compass.status & INV_SENSOR_ON) {
- if (timestamp < sensors.compass.timestamp) {
- timestamp = sensors.compass.timestamp;
- }
- }
- if (sensors.temp.status & INV_SENSOR_ON) {
- if (timestamp < sensors.temp.timestamp)
- timestamp = sensors.temp.timestamp;
- }
- return timestamp;
-}
-
-/** Sets the orientation and sensitivity of the gyro data.
-* @param[in] orientation A scalar defining the transformation from chip mounting
-* to the body frame. The function inv_orientation_matrix_to_scalar()
-* can convert the transformation matrix to this scalar and describes the
-* scalar in further detail.
-* @param[in] sensitivity A scale factor to convert device units to g's
-* such that g's = device_units * sensitivity / 2^30. Typically
-* it works out to be the maximum g_value * 2^15.
-*/
-void inv_set_accel_orientation_and_scale(int orientation, long sensitivity)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_A_ORIENT;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
- fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
- }
-#endif
- set_sensor_orientation_and_scale(&sensors.accel, orientation,
- sensitivity);
-}
-
-/** Sets the Orientation and Sensitivity of the gyro data.
-* @param[in] orientation A scalar defining the transformation from chip mounting
-* to the body frame. The function inv_orientation_matrix_to_scalar()
-* can convert the transformation matrix to this scalar and describes the
-* scalar in further detail.
-* @param[in] sensitivity A scale factor to convert device units to uT
-* such that uT = device_units * sensitivity / 2^30. Typically
-* it works out to be the maximum uT_value * 2^15.
-*/
-void inv_set_compass_orientation_and_scale(int orientation, long sensitivity)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_C_ORIENT;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file);
- fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file);
- }
-#endif
- set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity);
-}
-
-void inv_matrix_vector_mult(const long *A, const long *x, long *y)
-{
- y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]);
- y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]);
- y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]);
-}
-
-/** Takes raw data stored in the sensor, removes bias, and converts it to
-* calibrated data in the body frame. Also store raw data for body frame.
-* @param[in,out] sensor structure to modify
-* @param[in] bias bias in the mounting frame, in hardware units scaled by
-* 2^16. Length 3.
-*/
-void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
-{
- long raw32[3];
-
- // Convert raw to calibrated
- raw32[0] = (long)sensor->raw[0] << 15;
- raw32[1] = (long)sensor->raw[1] << 15;
- raw32[2] = (long)sensor->raw[2] << 15;
-
- inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
-
- raw32[0] -= bias[0] >> 1;
- raw32[1] -= bias[1] >> 1;
- raw32[2] -= bias[2] >> 1;
-
- inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
-
- sensor->status |= INV_CALIBRATED;
-}
-
-/** Returns the current bias for the compass
-* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame.
-* Length 3.
-*/
-void inv_get_compass_bias(long *bias)
-{
- if (bias != NULL) {
- memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias));
- }
-}
-
-void inv_set_compass_bias(const long *bias, int accuracy)
-{
- if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) {
- memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias));
- inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
- }
- sensors.compass.accuracy = accuracy;
- inv_data_builder.save.compass_accuracy = accuracy;
- inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
-}
-
-/** Set the state of a compass disturbance
-* @param[in] dist 1=disturbance, 0=no disturbance
-*/
-void inv_set_compass_disturbance(int dist)
-{
- inv_data_builder.compass_disturbance = dist;
-}
-
-int inv_get_compass_disturbance(void) {
- return inv_data_builder.compass_disturbance;
-}
-/** Sets the accel bias.
-* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
-* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
-*/
-void inv_set_accel_bias(const long *bias, int accuracy)
-{
- if (bias) {
- if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) {
- memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias));
- inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
- }
- }
- sensors.accel.accuracy = accuracy;
- inv_data_builder.save.accel_accuracy = accuracy;
- inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
-}
-
-/** Sets the accel accuracy.
-* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
-*/
-void inv_set_accel_accuracy(int accuracy)
-{
- sensors.accel.accuracy = accuracy;
- inv_data_builder.save.accel_accuracy = accuracy;
- inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
-}
-
-/** Sets the accel bias with control over which axis.
-* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame
-* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
-* @param[in] mask Mask to select axis to apply bias set.
-*/
-void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
-{
- if (bias) {
- if (mask & 1){
- inv_data_builder.save.accel_bias[0] = bias[0];
- }
- if (mask & 2){
- inv_data_builder.save.accel_bias[1] = bias[1];
- }
- if (mask & 4){
- inv_data_builder.save.accel_bias[2] = bias[2];
- }
-
- inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
- }
- sensors.accel.accuracy = accuracy;
- inv_data_builder.save.accel_accuracy = accuracy;
- inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
-}
-
-
-/** Sets the gyro bias
-* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame.
-* Length 3.
-* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate.
-*/
-void inv_set_gyro_bias(const long *bias, int accuracy)
-{
- if (bias != NULL) {
- if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) {
- memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias));
- inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias);
- }
- }
- sensors.gyro.accuracy = accuracy;
- inv_data_builder.save.gyro_accuracy = accuracy;
-
- /* TODO: What should we do if there's no temperature data? */
- if (sensors.temp.calibrated[0])
- inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
- else
- /* Set to 27 deg C for now until we've got a better solution. */
- inv_data_builder.save.gyro_temp = 1769472L;
- inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0);
-}
-
-/* TODO: Add this information to inv_sensor_cal_t */
-/**
- * Get the gyro biases and temperature record from MPL
- * @param[in] bias
- * Gyro bias in hardware units scaled by 2^16.
- * In chip mounting frame.
- * Length 3.
- * @param[in] temp
- * Tempearature in degrees C.
- */
-void inv_get_gyro_bias(long *bias, long *temp)
-{
- if (bias != NULL)
- memcpy(bias, inv_data_builder.save.gyro_bias,
- sizeof(inv_data_builder.save.gyro_bias));
- if (temp != NULL)
- temp[0] = inv_data_builder.save.gyro_temp;
-}
-
-/** Get Accel Bias
-* @param[out] bias Accel bias where
-* @param[out] temp Temperature where 1 C = 2^16
-*/
-void inv_get_accel_bias(long *bias, long *temp)
-{
- if (bias != NULL)
- memcpy(bias, inv_data_builder.save.accel_bias,
- sizeof(inv_data_builder.save.accel_bias));
- if (temp != NULL)
- temp[0] = inv_data_builder.save.accel_temp;
-}
-
-/**
- * Record new accel data for use when inv_execute_on_data() is called
- * @param[in] accel accel data.
- * Length 3.
- * Calibrated data is in m/s^2 scaled by 2^16 in body frame.
- * Raw data is in device units in chip mounting frame.
- * @param[in] status
- * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3
- * being most accurate.
- * The upper bit INV_CALIBRATED, is set if the data was calibrated
- * outside MPL and it is not set if the data being passed is raw.
- * Raw data should be in device units, typically in a 16-bit range.
- * @param[in] timestamp
- * Monotonic time stamp, for Android it's in nanoseconds.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_ACCEL;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file);
- fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file);
- }
-#endif
-
- if ((status & INV_CALIBRATED) == 0) {
- sensors.accel.raw[0] = (short)accel[0];
- sensors.accel.raw[1] = (short)accel[1];
- sensors.accel.raw[2] = (short)accel[2];
- sensors.accel.status |= INV_RAW_DATA;
- inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
- } else {
- sensors.accel.calibrated[0] = accel[0];
- sensors.accel.calibrated[1] = accel[1];
- sensors.accel.calibrated[2] = accel[2];
- sensors.accel.status |= INV_CALIBRATED;
- sensors.accel.accuracy = status & 3;
- inv_data_builder.save.accel_accuracy = status & 3;
- }
- sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
- sensors.accel.timestamp_prev = sensors.accel.timestamp;
- sensors.accel.timestamp = timestamp;
-
- return INV_SUCCESS;
-}
-
-/** Record new gyro data and calls inv_execute_on_data() if previous
-* sample has not been processed.
-* @param[in] gyro Data is in device units. Length 3.
-* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
-* @param[out] executed Set to 1 if data processing was done.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_GYRO;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file);
- fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file);
- }
-#endif
-
- memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short));
- sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
- sensors.gyro.timestamp_prev = sensors.gyro.timestamp;
- sensors.gyro.timestamp = timestamp;
- inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias);
-
- return INV_SUCCESS;
-}
-
-/** Record new compass data for use when inv_execute_on_data() is called
-* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16.
-* Length 3.
-* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate.
-* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is
-* not set if the data being passed is raw. Raw data should be in device units, typically
-* in a 16-bit range.
-* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds.
-* @param[out] executed Set to 1 if data processing was done.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_build_compass(const long *compass, int status,
- inv_time_t timestamp)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_COMPASS;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file);
- fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file);
- }
-#endif
-
- if ((status & INV_CALIBRATED) == 0) {
- sensors.compass.raw[0] = (short)compass[0];
- sensors.compass.raw[1] = (short)compass[1];
- sensors.compass.raw[2] = (short)compass[2];
- inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
- sensors.compass.status |= INV_RAW_DATA;
- } else {
- sensors.compass.calibrated[0] = compass[0];
- sensors.compass.calibrated[1] = compass[1];
- sensors.compass.calibrated[2] = compass[2];
- sensors.compass.status |= INV_CALIBRATED;
- sensors.compass.accuracy = status & 3;
- inv_data_builder.save.compass_accuracy = status & 3;
- }
- sensors.compass.timestamp_prev = sensors.compass.timestamp;
- sensors.compass.timestamp = timestamp;
- sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON;
-
- return INV_SUCCESS;
-}
-
-/** Record new temperature data for use when inv_execute_on_data() is called.
- * @param[in] temp Temperature data in q16 format.
- * @param[in] timestamp Monotonic time stamp; for Android it's in
- * nanoseconds.
-* @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_build_temp(const long temp, inv_time_t timestamp)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_TEMPERATURE;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(&temp, sizeof(temp), 1, inv_data_builder.file);
- fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file);
- }
-#endif
- sensors.temp.calibrated[0] = temp;
- sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
- sensors.temp.timestamp_prev = sensors.temp.timestamp;
- sensors.temp.timestamp = timestamp;
- /* TODO: Apply scale, remove offset. */
-
- return INV_SUCCESS;
-}
-/** quaternion data
-* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data.
-* Real part first. Length 4.
-* @param[in] status number of axis, 16-bit or 32-bit
-* @param[in] timestamp
-* @param[in] timestamp Monotonic time stamp; for Android it's in
-* nanoseconds.
-* @param[out] executed Set to 1 if data processing was done.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp)
-{
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_QUAT;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file);
- fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file);
- }
-#endif
-
- memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
- sensors.quat.timestamp = timestamp;
- sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
- sensors.quat.status |= (INV_BIAS_APPLIED & status);
-
- return INV_SUCCESS;
-}
-
-/** This should be called when the accel has been turned off. This is so
-* that we will know if the data is contiguous.
-*/
-void inv_accel_was_turned_off()
-{
- sensors.accel.status = 0;
-}
-
-/** This should be called when the compass has been turned off. This is so
-* that we will know if the data is contiguous.
-*/
-void inv_compass_was_turned_off()
-{
- sensors.compass.status = 0;
-}
-
-/** This should be called when the quaternion data from the DMP has been turned off. This is so
-* that we will know if the data is contiguous.
-*/
-void inv_quaternion_sensor_was_turned_off(void)
-{
- sensors.quat.status = 0;
-}
-
-/** This should be called when the gyro has been turned off. This is so
-* that we will know if the data is contiguous.
-*/
-void inv_gyro_was_turned_off()
-{
- sensors.gyro.status = 0;
-}
-
-/** This should be called when the temperature sensor has been turned off.
- * This is so that we will know if the data is contiguous.
- */
-void inv_temperature_was_turned_off()
-{
- sensors.temp.status = 0;
-}
-
-/** Registers to receive a callback when there is new sensor data.
-* @internal
-* @param[in] func Function pointer to receive callback when there is new sensor data
-* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
-* numbers must be unique.
-* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
-* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
-* gyro data, INV_MAG_NEW = compass data. So passing in
-* INV_ACCEL_NEW | INV_MAG_NEW, a
-* callback would be generated if there was new magnetomer data OR new accel data.
-*/
-inv_error_t inv_register_data_cb(
- inv_error_t (*func)(struct inv_sensor_cal_t *data),
- int priority, int sensor_type)
-{
- inv_error_t result = INV_SUCCESS;
- int kk, nn;
-
- // Make sure we haven't registered this function already
- // Or used the same priority
- for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
- if ((inv_data_builder.process[kk].func == func) ||
- (inv_data_builder.process[kk].priority == priority)) {
- return INV_ERROR_INVALID_PARAMETER; //fixme give a warning
- }
- }
-
- // Make sure we have not filled up our number of allowable callbacks
- if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) {
- kk = 0;
- if (inv_data_builder.num_cb != 0) {
- // set kk to be where this new callback goes in the array
- while ((kk < inv_data_builder.num_cb) &&
- (inv_data_builder.process[kk].priority < priority)) {
- kk++;
- }
- if (kk != inv_data_builder.num_cb) {
- // We need to move the others
- for (nn = inv_data_builder.num_cb; nn > kk; --nn) {
- inv_data_builder.process[nn] =
- inv_data_builder.process[nn - 1];
- }
- }
- }
- // Add new callback
- inv_data_builder.process[kk].func = func;
- inv_data_builder.process[kk].priority = priority;
- inv_data_builder.process[kk].data_required = sensor_type;
- inv_data_builder.num_cb++;
- } else {
- MPL_LOGE("Unable to add feature callback as too many were already registered\n");
- result = INV_ERROR_MEMORY_EXAUSTED;
- }
-
- return result;
-}
-
-/** Unregisters the callback that happens when new sensor data is received.
-* @internal
-* @param[in] func Function pointer to receive callback when there is new sensor data
-* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority
-* numbers must be unique.
-* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be
-* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW =
-* gyro data, INV_MAG_NEW = compass data. So passing in
-* INV_ACCEL_NEW | INV_MAG_NEW, a
-* callback would be generated if there was new magnetomer data OR new accel data.
-*/
-inv_error_t inv_unregister_data_cb(
- inv_error_t (*func)(struct inv_sensor_cal_t *data))
-{
- int kk, nn;
-
- for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
- if (inv_data_builder.process[kk].func == func) {
- // Delete this callback
- for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) {
- inv_data_builder.process[nn - 1] =
- inv_data_builder.process[nn];
- }
- inv_data_builder.num_cb--;
- return INV_SUCCESS;
- }
- }
-
- return INV_SUCCESS; // We did not find the callback
-}
-
-/** After at least one of inv_build_gyro(), inv_build_accel(), or
-* inv_build_compass() has been called, this function should be called.
-* It will process the data it has received and update all the internal states
-* and features that have been turned on.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_execute_on_data(void)
-{
- inv_error_t result, first_error;
- int kk;
-
-#ifdef INV_PLAYBACK_DBG
- if (inv_data_builder.debug_mode == RD_RECORD) {
- int type = PLAYBACK_DBG_TYPE_EXECUTE;
- fwrite(&type, sizeof(type), 1, inv_data_builder.file);
- }
-#endif
- // Determine what new data we have
- inv_data_builder.mode = 0;
- if (sensors.gyro.status & INV_NEW_DATA)
- inv_data_builder.mode |= INV_GYRO_NEW;
- if (sensors.accel.status & INV_NEW_DATA)
- inv_data_builder.mode |= INV_ACCEL_NEW;
- if (sensors.compass.status & INV_NEW_DATA)
- inv_data_builder.mode |= INV_MAG_NEW;
- if (sensors.temp.status & INV_NEW_DATA)
- inv_data_builder.mode |= INV_TEMP_NEW;
- if (sensors.quat.status & INV_QUAT_NEW)
- inv_data_builder.mode |= INV_QUAT_NEW;
-
- first_error = INV_SUCCESS;
-
- for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
- if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) {
- result = inv_data_builder.process[kk].func(&sensors);
- if (result && !first_error) {
- first_error = result;
- }
- }
- }
-
- inv_set_contiguous();
-
- return first_error;
-}
-
-/** Cleans up status bits after running all the callbacks. It sets the contiguous flag.
-*
-*/
-static void inv_set_contiguous(void)
-{
- inv_time_t current_time = 0;
- if (sensors.gyro.status & INV_NEW_DATA) {
- sensors.gyro.status |= INV_CONTIGUOUS;
- current_time = sensors.gyro.timestamp;
- }
- if (sensors.accel.status & INV_NEW_DATA) {
- sensors.accel.status |= INV_CONTIGUOUS;
- current_time = MAX(current_time, sensors.accel.timestamp);
- }
- if (sensors.compass.status & INV_NEW_DATA) {
- sensors.compass.status |= INV_CONTIGUOUS;
- current_time = MAX(current_time, sensors.compass.timestamp);
- }
- if (sensors.temp.status & INV_NEW_DATA) {
- sensors.temp.status |= INV_CONTIGUOUS;
- current_time = MAX(current_time, sensors.temp.timestamp);
- }
- if (sensors.quat.status & INV_NEW_DATA) {
- sensors.quat.status |= INV_CONTIGUOUS;
- current_time = MAX(current_time, sensors.quat.timestamp);
- }
-
-#if 0
- /* See if sensors are still on. These should be turned off by inv_*_was_turned_off()
- * type functions. This is just in case that breaks down. We make sure
- * all the data is within 2 seconds of the newest piece of data*/
- if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000)
- inv_gyro_was_turned_off();
- if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000)
- inv_accel_was_turned_off();
- if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000)
- inv_compass_was_turned_off();
- /* TODO: Temperature might not need to be read this quickly. */
- if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000)
- inv_temperature_was_turned_off();
-#endif
-
- /* clear bits */
- sensors.gyro.status &= ~INV_NEW_DATA;
- sensors.accel.status &= ~INV_NEW_DATA;
- sensors.compass.status &= ~INV_NEW_DATA;
- sensors.temp.status &= ~INV_NEW_DATA;
- sensors.quat.status &= ~INV_NEW_DATA;
-}
-
-/** Gets a whole set of accel data including data, accuracy and timestamp.
- * @param[out] data Accel Data where 1g = 2^16
- * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
- * @param[out] timestamp The timestamp of the data sample.
-*/
-void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
-{
- if (data != NULL) {
- memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated));
- }
- if (timestamp != NULL) {
- *timestamp = sensors.accel.timestamp;
- }
- if (accuracy != NULL) {
- *accuracy = sensors.accel.accuracy;
- }
-}
-
-/** Gets a whole set of gyro data including data, accuracy and timestamp.
- * @param[out] data Gyro Data where 1 dps = 2^16
- * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
- * @param[out] timestamp The timestamp of the data sample.
-*/
-void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
-{
- memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
- if (timestamp != NULL) {
- *timestamp = sensors.gyro.timestamp;
- }
- if (accuracy != NULL) {
- *accuracy = sensors.gyro.accuracy;
- }
-}
-
-/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
- * @param[out] data Gyro Data where 1 dps = 2^16
- * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
- * @param[out] timestamp The timestamp of the data sample.
-*/
-void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
-{
- memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
- if (timestamp != NULL) {
- *timestamp = sensors.gyro.timestamp;
- }
- if (accuracy != NULL) {
- *accuracy = sensors.gyro.accuracy;
- }
-}
-
-/** Get's latest gyro data.
-* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
-*/
-void inv_get_gyro(long *gyro)
-{
- memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
-}
-
-/** Gets a whole set of compass data including data, accuracy and timestamp.
- * @param[out] data Compass Data where 1 uT = 2^16
- * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
- * @param[out] timestamp The timestamp of the data sample.
-*/
-void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
-{
- memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated));
- if (timestamp != NULL) {
- *timestamp = sensors.compass.timestamp;
- }
- if (accuracy != NULL) {
- if (inv_data_builder.compass_disturbance)
- *accuracy = 0;
- else
- *accuracy = sensors.compass.accuracy;
- }
-}
-
-/** Gets a whole set of temperature data including data, accuracy and timestamp.
- * @param[out] data Temperature data where 1 degree C = 2^16
- * @param[out] accuracy 0 to 3, where 3 is most accurate.
- * @param[out] timestamp The timestamp of the data sample.
- */
-void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp)
-{
- data[0] = sensors.temp.calibrated[0];
- if (timestamp)
- *timestamp = sensors.temp.timestamp;
- if (accuracy)
- *accuracy = sensors.temp.accuracy;
-}
-
-/** Returns accuracy of gyro.
- * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate.
-*/
-int inv_get_gyro_accuracy(void)
-{
- return sensors.gyro.accuracy;
-}
-
-/** Returns accuracy of compass.
- * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate.
-*/
-int inv_get_mag_accuracy(void)
-{
- if (inv_data_builder.compass_disturbance)
- return 0;
- return sensors.compass.accuracy;
-}
-
-/** Returns accuracy of accel.
- * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate.
-*/
-int inv_get_accel_accuracy(void)
-{
- return sensors.accel.accuracy;
-}
-
-inv_error_t inv_get_gyro_orient(int *orient)
-{
- *orient = sensors.gyro.orientation;
- return 0;
-}
-
-inv_error_t inv_get_accel_orient(int *orient)
-{
- *orient = sensors.accel.orientation;
- return 0;
-}
-
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h
deleted file mode 100644
index c8c18cf..0000000
--- a/60xx/libsensors_iio/software/core/mllite/data_builder.h
+++ /dev/null
@@ -1,240 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#include "mltypes.h"
-
-#ifndef INV_DATA_BUILDER_H__
-#define INV_DATA_BUILDER_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// Uncomment this flag to enable playback debug and record or playback scenarios
-//#define INV_PLAYBACK_DBG
-
-/** This is a new sample of accel data */
-#define INV_ACCEL_NEW 1
-/** This is a new sample of gyro data */
-#define INV_GYRO_NEW 2
-/** This is a new sample of compass data */
-#define INV_MAG_NEW 4
-/** This is a new sample of temperature data */
-#define INV_TEMP_NEW 8
-/** This is a new sample of quaternion data */
-#define INV_QUAT_NEW 16
-/** Set if quaternion is 6-axis from DMP */
-#define INV_QUAT_6AXIS 1024
-/** Set if quaternion is 3-axis from DMP */
-#define INV_QUAT_3AXIS 4096
-
-/** Set if the data is contiguous. Typically not set if a sample was skipped */
-#define INV_CONTIGUOUS 16
-/** Set if the calibrated data has been solved for */
-#define INV_CALIBRATED 32
-/* INV_NEW_DATA set for a new set of data, cleared if not available. */
-#define INV_NEW_DATA 64
-/* Set if raw data exists */
-#define INV_RAW_DATA 128
-/* Set if the sensor is on */
-#define INV_SENSOR_ON 256
-/* Set if quaternion has bias correction applied */
-#define INV_BIAS_APPLIED 512
-
-#define INV_PRIORITY_MOTION_NO_MOTION 100
-#define INV_PRIORITY_GYRO_TC 150
-#define INV_PRIORITY_QUATERNION_GYRO_ACCEL 200
-#define INV_PRIORITY_QUATERNION_NO_GYRO 250
-#define INV_PRIORITY_MAGNETIC_DISTURBANCE 300
-#define INV_PRIORITY_HEADING_FROM_GYRO 350
-#define INV_PRIORITY_COMPASS_BIAS_W_GYRO 375
-#define INV_PRIORITY_COMPASS_VECTOR_CAL 400
-#define INV_PRIORITY_COMPASS_ADV_BIAS 500
-#define INV_PRIORITY_9_AXIS_FUSION 600
-#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS 700
-#define INV_PRIORITY_QUATERNION_ACCURACY 750
-#define INV_PRIORITY_RESULTS_HOLDER 800
-#define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850
-#define INV_PRIORITY_HAL_OUTPUTS 900
-#define INV_PRIORITY_GLYPH 950
-#define INV_PRIORITY_SHAKE 975
-#define INV_PRIORITY_SM 1000
-
-struct inv_single_sensor_t {
- /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when
- * the rotation matrix could be thought of only having elements of 0,1,-1.
- * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign.
- * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row.
- * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row.
- * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row.
- */
- int orientation;
- /** The raw data in raw data units in the mounting frame */
- short raw[3];
- /** Raw data in body frame */
- long raw_scaled[3];
- /** Calibrated data */
- long calibrated[3];
- long sensitivity;
- /** Sample rate in microseconds */
- long sample_rate_us;
- long sample_rate_ms;
- /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
- * skipped due to power savings turning off this sensor.
- * INV_NEW_DATA set for a new set of data, cleared if not available.
- * INV_CALIBRATED_SET if calibrated data has been solved for */
- int status;
- /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */
- int accuracy;
- inv_time_t timestamp;
- inv_time_t timestamp_prev;
- /** Bandwidth in Hz */
- int bandwidth;
-};
-struct inv_quat_sensor_t {
- long raw[4];
- /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample
- * skipped due to power savings turning off this sensor.
- * INV_NEW_DATA set for a new set of data, cleared if not available.
- * INV_CALIBRATED_SET if calibrated data has been solved for */
- int status;
- inv_time_t timestamp;
- inv_time_t timestamp_prev;
- long sample_rate_us;
- long sample_rate_ms;
-};
-
-struct inv_sensor_cal_t {
- struct inv_single_sensor_t gyro;
- struct inv_single_sensor_t accel;
- struct inv_single_sensor_t compass;
- struct inv_single_sensor_t temp;
- struct inv_quat_sensor_t quat;
- /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate
- * which data is a new sample as these data points may have different sample rates.
- */
- int status;
-};
-
-// Useful for debug record and playback
-typedef enum {
- RD_NO_DEBUG,
- RD_RECORD,
- RD_PLAYBACK
-} rd_dbg_mode;
-
-typedef enum {
- PLAYBACK_DBG_TYPE_GYRO,
- PLAYBACK_DBG_TYPE_ACCEL,
- PLAYBACK_DBG_TYPE_COMPASS,
- PLAYBACK_DBG_TYPE_TEMPERATURE,
- PLAYBACK_DBG_TYPE_EXECUTE,
- PLAYBACK_DBG_TYPE_A_ORIENT,
- PLAYBACK_DBG_TYPE_G_ORIENT,
- PLAYBACK_DBG_TYPE_C_ORIENT,
- PLAYBACK_DBG_TYPE_A_SAMPLE_RATE,
- PLAYBACK_DBG_TYPE_C_SAMPLE_RATE,
- PLAYBACK_DBG_TYPE_G_SAMPLE_RATE,
- PLAYBACK_DBG_TYPE_GYRO_OFF,
- PLAYBACK_DBG_TYPE_ACCEL_OFF,
- PLAYBACK_DBG_TYPE_COMPASS_OFF,
- PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE,
- PLAYBACK_DBG_TYPE_QUAT
-
-} inv_rd_dbg_states;
-
-/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/
-#define INV_MAX_DATA_CB 20
-
-#ifdef INV_PLAYBACK_DBG
-#include <stdio.h>
-void inv_turn_on_data_logging(FILE *file);
-void inv_turn_off_data_logging();
-#endif
-
-void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity);
-void inv_set_accel_orientation_and_scale(int orientation,
- long sensitivity);
-void inv_set_compass_orientation_and_scale(int orientation,
- long sensitivity);
-void inv_set_gyro_sample_rate(long sample_rate_us);
-void inv_set_compass_sample_rate(long sample_rate_us);
-void inv_set_quat_sample_rate(long sample_rate_us);
-void inv_set_accel_sample_rate(long sample_rate_us);
-void inv_set_gyro_bandwidth(int bandwidth_hz);
-void inv_set_accel_bandwidth(int bandwidth_hz);
-void inv_set_compass_bandwidth(int bandwidth_hz);
-
-void inv_get_gyro_sample_rate_ms(long *sample_rate_ms);
-void inv_get_accel_sample_rate_ms(long *sample_rate_ms);
-void inv_get_compass_sample_rate_ms(long *sample_rate_ms);
-
-inv_error_t inv_register_data_cb(inv_error_t (*func)
- (struct inv_sensor_cal_t * data), int priority,
- int sensor_type);
-inv_error_t inv_unregister_data_cb(inv_error_t (*func)
- (struct inv_sensor_cal_t * data));
-
-inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp);
-inv_error_t inv_build_compass(const long *compass, int status,
- inv_time_t timestamp);
-inv_error_t inv_build_accel(const long *accel, int status,
- inv_time_t timestamp);
-inv_error_t inv_build_temp(const long temp, inv_time_t timestamp);
-inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp);
-inv_error_t inv_execute_on_data(void);
-
-void inv_get_compass_bias(long *bias);
-
-void inv_set_compass_bias(const long *bias, int accuracy);
-void inv_set_compass_disturbance(int dist);
-void inv_set_gyro_bias(const long *bias, int accuracy);
-void inv_set_accel_bias(const long *bias, int accuracy);
-void inv_set_accel_accuracy(int accuracy);
-void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask);
-
-void inv_get_gyro_bias(long *bias, long *temp);
-void inv_get_accel_bias(long *bias, long *temp);
-
-void inv_gyro_was_turned_off(void);
-void inv_accel_was_turned_off(void);
-void inv_compass_was_turned_off(void);
-void inv_quaternion_sensor_was_turned_off(void);
-inv_error_t inv_init_data_builder(void);
-long inv_get_gyro_sensitivity(void);
-long inv_get_accel_sensitivity(void);
-long inv_get_compass_sensitivity(void);
-
-void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
-void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
-void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp);
-void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp);
-
-void inv_get_gyro(long *gyro);
-
-int inv_get_gyro_accuracy(void);
-int inv_get_accel_accuracy(void);
-int inv_get_mag_accuracy(void);
-
-int inv_get_compass_on(void);
-int inv_get_gyro_on(void);
-int inv_get_accel_on(void);
-
-inv_time_t inv_get_last_timestamp(void);
-int inv_get_compass_disturbance(void);
-
-//New DMP Cal Functions
-inv_error_t inv_get_gyro_orient(int *orient);
-inv_error_t inv_get_accel_orient(int *orient);
-
-// internal
-int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* INV_DATA_BUILDER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
deleted file mode 100644
index caa1db7..0000000
--- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
+++ /dev/null
@@ -1,497 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/**
- * @defgroup HAL_Outputs hal_outputs
- * @brief Motion Library - HAL Outputs
- * Sets up common outputs for HAL
- *
- * @{
- * @file hal_outputs.c
- * @brief HAL Outputs.
- */
-
-#include <string.h>
-
-#include "hal_outputs.h"
-#include "log.h"
-#include "ml_math_func.h"
-#include "mlmath.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "results_holder.h"
-
-struct hal_output_t {
- int accuracy_mag; /**< Compass accuracy */
-// int accuracy_gyro; /**< Gyro Accuracy */
-// int accuracy_accel; /**< Accel Accuracy */
- int accuracy_quat; /**< quat Accuracy */
-
- inv_time_t nav_timestamp;
- inv_time_t gam_timestamp;
-// inv_time_t accel_timestamp;
- inv_time_t mag_timestamp;
- long nav_quat[4];
- int gyro_status;
- int accel_status;
- int compass_status;
- int nine_axis_status;
- inv_biquad_filter_t lp_filter[3];
- float compass_float[3];
- long linear_acceleration_sample_rate_us;
- long gravity_sample_rate_us;
-};
-
-static struct hal_output_t hal_out;
-
-void inv_set_linear_acceleration_sample_rate(long sample_rate_us)
-{
- hal_out.linear_acceleration_sample_rate_us = sample_rate_us;
-}
-
-void inv_set_gravity_sample_rate(long sample_rate_us)
-{
- hal_out.gravity_sample_rate_us = sample_rate_us;
-}
-
-/** Acceleration (m/s^2) in body frame.
-* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
-* should return a vector of magnitude near 9.81 m/s^2
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_accel().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- int status;
- /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
- * So this 9.80665 / 2^16 */
-#define ACCEL_CONVERSION 0.000149637603759766f
- long accel[3];
- inv_get_accel_set(accel, accuracy, timestamp);
- values[0] = accel[0] * ACCEL_CONVERSION;
- values[1] = accel[1] * ACCEL_CONVERSION;
- values[2] = accel[2] * ACCEL_CONVERSION;
- if (hal_out.accel_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-/** Linear Acceleration (m/s^2) in Body Frame.
-* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
-* accel biases while at rest.
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_accel().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gravity[3], accel[3];
- inv_time_t timestamp1;
-
- inv_get_accel_set(accel, accuracy, ×tamp1);
- inv_get_gravity(gravity);
- accel[0] -= gravity[0] >> 14;
- accel[1] -= gravity[1] >> 14;
- accel[2] -= gravity[2] >> 14;
- values[0] = accel[0] * ACCEL_CONVERSION;
- values[1] = accel[1] * ACCEL_CONVERSION;
- values[2] = accel[2] * ACCEL_CONVERSION;
-
- return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp);
-}
-
-/** Gravity vector (m/s^2) in Body Frame.
-* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_accel().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gravity[3];
-
- *accuracy = (int8_t) hal_out.accuracy_quat;
- inv_get_gravity(gravity);
- values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
- values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
- values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
- return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp);
-}
-
-/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
- * So this is: pi / 2^16 / 180 */
-#define GYRO_CONVERSION 2.66316109007924e-007f
-
-/** Gyroscope calibrated data (rad/s) in body frame.
-* @param[out] values Rotation Rate in rad/sec.
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_gyro().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gyro[3];
- int status;
-
- inv_get_gyro_set(gyro, accuracy, timestamp);
- values[0] = gyro[0] * GYRO_CONVERSION;
- values[1] = gyro[1] * GYRO_CONVERSION;
- values[2] = gyro[2] * GYRO_CONVERSION;
- if (hal_out.gyro_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-/** Gyroscope raw data (rad/s) in body frame.
-* @param[out] values Rotation Rate in rad/sec.
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_gyro().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gyro[3];
- int status;
-
- inv_get_gyro_set_raw(gyro, accuracy, timestamp);
- values[0] = gyro[0] * GYRO_CONVERSION;
- values[1] = gyro[1] * GYRO_CONVERSION;
- values[2] = gyro[2] * GYRO_CONVERSION;
- if (hal_out.gyro_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-/**
-* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
-* The rotation vector represents the orientation of the device as a combination
-* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
-* around an axis {x, y, z}. <br>
-* The three elements of the rotation vector are
-* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
-* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
-* equal to the direction of the axis of rotation.
-*
-* The three elements of the rotation vector are equal to the last three components of a unit quaternion
-* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
-*
-* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
-* The reference coordinate system is defined as a direct orthonormal basis, where:
-
- -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
- -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
- -Z points towards the sky and is perpendicular to the ground.
-* @param[out] values Length 4.
-* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
-* @param[out] timestamp Timestamp. In (ns) for Android.
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- *accuracy = (int8_t) hal_out.accuracy_quat;
- *timestamp = hal_out.nav_timestamp;
-
- if (hal_out.nav_quat[0] >= 0) {
- values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
- values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
- values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
- values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
- } else {
- values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
- values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
- values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
- values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
- }
- values[4] = inv_get_heading_confidence_interval();
-
- return hal_out.nine_axis_status;
-}
-
-
-/** Compass data (uT) in body frame.
-* @param[out] values Compass data in (uT), length 3. May be calibrated by having
-* biases removed and sensitivity adjusted
-* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
-* @param[out] timestamp Timestamp. In (ns) for Android.
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- int status;
- /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
- * So this is: 1 / 2^16*/
-//#define COMPASS_CONVERSION 1.52587890625e-005f
- int i;
-
- *timestamp = hal_out.mag_timestamp;
- *accuracy = (int8_t) hal_out.accuracy_mag;
-
- for (i=0; i<3; i++) {
- values[i] = hal_out.compass_float[i];
- }
- if (hal_out.compass_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-static void inv_get_rotation(float r[3][3])
-{
- long rot[9];
- float conv = 1.f / (1L<<30);
-
- inv_quaternion_to_rotation(hal_out.nav_quat, rot);
- r[0][0] = rot[0]*conv;
- r[0][1] = rot[1]*conv;
- r[0][2] = rot[2]*conv;
- r[1][0] = rot[3]*conv;
- r[1][1] = rot[4]*conv;
- r[1][2] = rot[5]*conv;
- r[2][0] = rot[6]*conv;
- r[2][1] = rot[7]*conv;
- r[2][2] = rot[8]*conv;
-}
-
-static void google_orientation(float *g)
-{
- float rad2deg = (float)(180.0 / M_PI);
- float R[3][3];
-
- inv_get_rotation(R);
-
- g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
- g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
- g[2] = asinf ( R[2][0]) * rad2deg;
- if (g[0] < 0)
- g[0] += 360;
-}
-
-
-/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
-* @param[out] values Length 3, Degrees.<br>
-* - values[0]: Azimuth, angle between the magnetic north direction
-* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
-* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
-* when the z-axis moves toward the y-axis.<br>
-* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
-* values when the x-axis moves toward the z-axis.<br>
-*
-* @note This definition is different from yaw, pitch and roll used in aviation
-* where the X axis is along the long side of the plane (tail to nose).
-* Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
-* in conjunction with remapCoordinateSystem() and getOrientation() to compute
-* these values instead.
-* Important note: For historical reasons the roll angle is positive in the
-* clockwise direction (mathematically speaking, it should be positive in
-* the counter-clockwise direction).
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor.
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- *accuracy = (int8_t) hal_out.accuracy_quat;
- *timestamp = hal_out.nav_timestamp;
-
- google_orientation(values);
-
- return hal_out.nine_axis_status;
-}
-
-/** Main callback to generate HAL outputs. Typically not called by library users.
-* @param[in] sensor_cal Input variable to take sensor data whenever there is new
-* sensor data.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
-{
- int use_sensor = 0;
- long sr = 1000;
- long compass[3];
- int8_t accuracy;
- int i;
- (void) sensor_cal;
-
- inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
- &hal_out.nav_timestamp);
- hal_out.gyro_status = sensor_cal->gyro.status;
- hal_out.accel_status = sensor_cal->accel.status;
- hal_out.compass_status = sensor_cal->compass.status;
-
- // Find the highest sample rate and tie generating 9-axis to that one.
- if (sensor_cal->gyro.status & INV_SENSOR_ON) {
- sr = sensor_cal->gyro.sample_rate_ms;
- use_sensor = 0;
- }
- if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {
- sr = sensor_cal->accel.sample_rate_ms;
- use_sensor = 1;
- }
- if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {
- sr = sensor_cal->compass.sample_rate_ms;
- use_sensor = 2;
- }
- if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {
- sr = sensor_cal->quat.sample_rate_ms;
- use_sensor = 3;
- }
-
- // Only output 9-axis if all 9 sensors are on.
- if (sensor_cal->quat.status & INV_SENSOR_ON) {
- // If quaternion sensor is on, gyros are not required as quaternion already has that part
- if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
- use_sensor = -1;
- }
- } else {
- if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
- use_sensor = -1;
- }
- }
-
- switch (use_sensor) {
- case 0:
- hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
- break;
- case 1:
- hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->accel.timestamp;
- break;
- case 2:
- hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->compass.timestamp;
- break;
- case 3:
- hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->quat.timestamp;
- break;
- default:
- hal_out.nine_axis_status = 0; // Don't output quaternion related info
- break;
- }
-
- /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
- * So this is: 1 / 2^16*/
- #define COMPASS_CONVERSION 1.52587890625e-005f
-
- inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
- hal_out.accuracy_mag = (int ) accuracy;
-
- for (i=0; i<3; i++) {
- if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
- INV_NEW_DATA ) {
- // set the state variables to match output with input
- inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);
- }
-
- if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
- (INV_NEW_DATA | INV_RAW_DATA) ) {
-
- hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],
- (float ) compass[i]) * COMPASS_CONVERSION;
-
- } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) {
- hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;
- }
-
- }
- return INV_SUCCESS;
-}
-
-/** Turns off generation of HAL outputs.
-* @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_stop_hal_outputs(void)
-{
- inv_error_t result;
- result = inv_unregister_data_cb(inv_generate_hal_outputs);
- return result;
-}
-
-/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()
-* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_start_hal_outputs(void)
-{
- inv_error_t result;
- result =
- inv_register_data_cb(inv_generate_hal_outputs,
- INV_PRIORITY_HAL_OUTPUTS,
- INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
- return result;
-}
-/* file name: lowPassFilterCoeff_1_6.c */
-float compass_low_pass_filter_coeff[5] =
-{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
-
-/** Initializes hal outputs class. This is called automatically by the
-* enable function. It may be called any time the feature is enabled, but
-* is typically not needed to be called by outside callers.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_hal_outputs(void)
-{
- int i;
- memset(&hal_out, 0, sizeof(hal_out));
- for (i=0; i<3; i++) {
- inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
- }
-
- return INV_SUCCESS;
-}
-
-/** Turns on creation and storage of HAL type results.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_enable_hal_outputs(void)
-{
- inv_error_t result;
-
- // don't need to check the result for inv_init_hal_outputs
- // since it's always INV_SUCCESS
- inv_init_hal_outputs();
-
- result = inv_register_mpl_start_notification(inv_start_hal_outputs);
- return result;
-}
-
-/** Turns off creation and storage of HAL type results.
-*/
-inv_error_t inv_disable_hal_outputs(void)
-{
- inv_error_t result;
-
- inv_stop_hal_outputs(); // Ignore error if we have already stopped this
- result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
- return result;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
deleted file mode 100644
index 41b72c6..0000000
--- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#include "mltypes.h"
-
-#ifndef INV_HAL_OUTPUTS_H__
-#define INV_HAL_OUTPUTS_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
- int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
- int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
- int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
- int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
- int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
-
- int inv_get_sensor_type_linear_acceleration(float *values,
- int8_t *accuracy,
- inv_time_t * timestamp);
- int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
- inv_time_t * timestamp);
-
- inv_error_t inv_enable_hal_outputs(void);
- inv_error_t inv_disable_hal_outputs(void);
- inv_error_t inv_init_hal_outputs(void);
- inv_error_t inv_start_hal_outputs(void);
- inv_error_t inv_stop_hal_outputs(void);
-
- // Set data rates for virtual sensors
- void inv_set_linear_acceleration_sample_rate(long sample_rate_us);
- void inv_set_gravity_sample_rate(long sample_rate_us);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_HAL_OUTPUTS_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h
deleted file mode 100644
index fb8e12b..0000000
--- a/60xx/libsensors_iio/software/core/mllite/invensense.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-/**
- * Main header file for Invensense's basic library.
- */
-
-#include "data_builder.h"
-#include "hal_outputs.h"
-#include "message_layer.h"
-#include "mlmath.h"
-#include "ml_math_func.h"
-#include "mpl.h"
-#include "results_holder.h"
-#include "start_manager.h"
-#include "storage_manager.h"
-#include "log.h"
-#include "mlinclude.h"
-//#include "..\HAL\include\mlos.h"
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
deleted file mode 100644
index 72940f7..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c
+++ /dev/null
@@ -1,279 +0,0 @@
-/*
- $License:
- Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/******************************************************************************
- *
- * $Id:$
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML_LOAD_DMP
- *
- * @{
- * @file ml_load_dmp.c
- * @brief functions for writing dmp firmware.
- */
-#include <stdio.h>
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-loaddmp"
-
-#include "ml_load_dmp.h"
-#include "mlos.h"
-
-#define LOADDMP_LOG MPL_LOGI
-#define LOADDMP_LOG MPL_LOGI
-
-#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
-#define DMP_CODE_SIZE 3065
-
-static const unsigned char dmpMemory[DMP_CODE_SIZE] = {
- /* bank # 0 */
- 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
- 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
- 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
- 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
- 0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
- 0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02,
- 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
- 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
- 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
- 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
- 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
- 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
- 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
- 0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8,
- 0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8,
- /* bank # 1 */
- 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
- 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
- 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
- 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
- 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
- 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
- 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
- 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
- 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
- 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
- /* bank # 2 */
- 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
- 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
- 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2,
- 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
- 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
- 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
- 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- /* bank # 3 */
- 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
- 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-
- /* bank # 4 */
- 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
- 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
- 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97,
- 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99,
- 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99,
- 0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0,
- 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18,
- 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93,
- 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3,
- 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68,
- 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba,
- 0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
- 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
- 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19,
- 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8,
- 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c,
- /* bank # 5 */
- 0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1,
- 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69,
- 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1,
- 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3,
- 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9,
- 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98,
- 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8,
- 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9,
- 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1,
- 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4,
- 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92,
- 0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e,
- 0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25,
- 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8,
- 0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95,
- 0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad,
- /* bank # 6 */
- 0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78,
- 0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31,
- 0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5,
- 0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e,
- 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9,
- 0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1,
- 0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8,
- 0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2,
- 0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1,
- 0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d,
- 0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb,
- 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf,
- 0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9,
- 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c,
- 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30,
- 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d,
- /* bank # 7 */
- 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0,
- 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8,
- 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9,
- 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8,
- 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c,
- 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9,
- 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0,
- 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38,
- 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0,
- 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf,
- 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2,
- 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9,
- 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda,
- 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae,
- 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9,
- 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4,
- /* bank # 8 */
- 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3,
- 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8,
- 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84,
- 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3,
- 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3,
- 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1,
- 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 0xb4, 0xb0,
- 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20,
- 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29,
- 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00,
- 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4,
- 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0,
- 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20,
- 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30,
- 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78,
- 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c,
- /* bank # 9 */
- 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09,
- 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c,
- 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99,
- 0x88, 0x2d, 0x55, 0x7d, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9,
- 0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50,
- 0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8,
- 0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58,
- 0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1,
- 0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f,
- 0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9,
- 0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65,
- 0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa,
- 0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0,
- 0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09,
- 0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4,
- 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60,
- /* bank # 10 */
- 0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a,
- 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
- 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
- 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
- 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
- 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
- 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
- 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
- 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09,
- 0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b,
- 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
- 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba,
- 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
- 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80,
- 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79,
- 0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85,
- /* bank # 11 */
- 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85,
- 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10,
- 0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8,
- 0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c,
- 0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89,
- 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95,
- 0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39,
- 0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda,
- 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda,
- 0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1,
- 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0,
- 0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde,
- 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96,
- 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c,
- 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35,
- 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
-};
-
-#define DMP_VERSION (dmpMemory)
-
-inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
-{
- inv_error_t result = INV_SUCCESS;
- int bytesWritten = 0;
-
- if (len <= 0) {
- MPL_LOGE("Nothing to write");
- return INV_ERROR_FILE_WRITE;
- } else {
- MPL_LOGI("dmp firmware size to write = %d", len);
- }
- if (fd == NULL) {
- return INV_ERROR_FILE_OPEN;
- }
- bytesWritten = fwrite(dmp, 1, len, fd);
- if (bytesWritten != len) {
- MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
- bytesWritten, len);
- result = INV_ERROR_FILE_WRITE;
- } else {
- MPL_LOGI("Bytes written = %d", bytesWritten);
- }
- return result;
-}
-
-inv_error_t inv_load_dmp(FILE *fd)
-{
- inv_error_t result = INV_SUCCESS;
- result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE);
- return result;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
deleted file mode 100644
index 4d98692..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- $License:
- Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-#ifndef INV_LOAD_DMP_H
-#define INV_LOAD_DMP_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
- Includes.
-*/
-#include "mltypes.h"
-
-/*
- APIs
-*/
-inv_error_t inv_load_dmp(FILE *fd);
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* INV_LOAD_DMP_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
deleted file mode 100644
index b4c4249..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c
+++ /dev/null
@@ -1,353 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/******************************************************************************
- *
- * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML_STORED_DATA
- *
- * @{
- * @file ml_stored_data.c
- * @brief functions for reading and writing stored data sets.
- * Typically, these functions process stored calibration data.
- */
-
-#include <stdio.h>
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-storeload"
-
-#include "ml_stored_data.h"
-#include "storage_manager.h"
-#include "mlos.h"
-
-#define LOADCAL_DEBUG 0
-#define STORECAL_DEBUG 0
-
-#define DEFAULT_KEY 29681
-
-#define STORECAL_LOG MPL_LOGI
-#define LOADCAL_LOG MPL_LOGI
-
-inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
-{
- FILE *fp;
- inv_error_t result = INV_SUCCESS;
- size_t fsize;
-
- fp = fopen(MLCAL_FILE,"rb");
- if (fp == NULL) {
- MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
- return INV_ERROR_FILE_OPEN;
- }
-
- // obtain file size
- fseek (fp, 0 , SEEK_END);
- fsize = ftell (fp);
- rewind (fp);
-
- *calData = (unsigned char *)inv_malloc(fsize);
- if (*calData==NULL) {
- MPL_LOGE("Could not allocate buffer of %d bytes - "
- "aborting\n", fsize);
- fclose(fp);
- return INV_ERROR_MEMORY_EXAUSTED;
- }
-
- *bytesRead = fread(*calData, 1, fsize, fp);
- if (*bytesRead != fsize) {
- MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
- *bytesRead, fsize);
- result = INV_ERROR_FILE_READ;
- goto read_cal_end;
- }
- else {
- MPL_LOGI("Bytes read = %d", *bytesRead);
- }
-
-read_cal_end:
- fclose(fp);
- return result;
-}
-
-inv_error_t inv_write_cal(unsigned char *cal, size_t len)
-{
- FILE *fp;
- int bytesWritten;
- inv_error_t result = INV_SUCCESS;
-
- if (len <= 0) {
- MPL_LOGE("Nothing to write");
- return INV_ERROR_FILE_WRITE;
- }
- else {
- MPL_LOGI("cal data size to write = %d", len);
- }
- fp = fopen(MLCAL_FILE,"wb");
- if (fp == NULL) {
- MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
- return INV_ERROR_FILE_OPEN;
- }
- bytesWritten = fwrite(cal, 1, len, fp);
- if (bytesWritten != len) {
- MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
- bytesWritten, len);
- result = INV_ERROR_FILE_WRITE;
- }
- else {
- MPL_LOGI("Bytes written = %d", bytesWritten);
- }
- fclose(fp);
- return result;
-}
-
-/**
- * @brief Loads a type 0 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature compensation : temperature data points,
- * - temperature compensation : gyro biases data points for X, Y,
- * and Z axes.
- * - accel biases for X, Y, Z axes.
- * This calibration data is produced internally by the MPL and its
- * size is 2777 bytes (header and checksum included).
- * Calibration format type 1 is currently used for ITG3500
- *
- * @pre inv_init_storage_manager()
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
-{
- inv_error_t result;
-
- LOADCAL_LOG("Entering inv_load_cal_V0\n");
-
- /*if (len != expLen) {
- MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
- expLen, len);
- return INV_ERROR_FILE_READ;
- }*/
-
- result = inv_load_mpl_states(calData, len);
- return result;
-}
-
-/**
- * @brief Loads a type 1 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature,
- * - gyro biases for X, Y, Z axes,
- * - accel biases for X, Y, Z axes.
- * This calibration data would normally be produced by the MPU Self
- * Test and its size is 36 bytes (header and checksum included).
- * Calibration format type 1 is produced by the MPU Self Test and
- * substitutes the type 0 : inv_load_cal_V0().
- *
- * @pre
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
-{
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- *
- * @pre
- *
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal(unsigned char *calData)
-{
- int calType = 0;
- int len = 0;
- //int ptr;
- //uint32_t chk = 0;
- //uint32_t cmp_chk = 0;
-
- /*load_func_t loaders[] = {
- inv_load_cal_V0,
- inv_load_cal_V1,
- };
- */
-
- inv_load_cal_V0(calData, len);
-
- /* read the header (type and len)
- len is the total record length including header and checksum */
- len = 0;
- len += 16777216L * ((int)calData[0]);
- len += 65536L * ((int)calData[1]);
- len += 256 * ((int)calData[2]);
- len += (int)calData[3];
-
- calType = ((int)calData[4]) * 256 + ((int)calData[5]);
- if (calType > 5) {
- MPL_LOGE("Unsupported calibration file format %d. "
- "Valid types 0..5\n", calType);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- /* call the proper method to read in the data */
- //return loaders[calType] (calData, len);
- return 0;
-}
-
-/**
- * @brief Stores a set of calibration data.
- * It generates a binary data set containing calibration data.
- * The binary data set is intended to be stored into a file.
- *
- * @pre inv_dmp_open()
- *
- * @param calData
- * A pointer to an array of bytes to be stored.
- * @param length
- * The amount of bytes available in the array.
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_store_cal(unsigned char *calData, size_t length)
-{
- inv_error_t res = 0;
- size_t size;
-
- STORECAL_LOG("Entering inv_store_cal\n");
-
- inv_get_mpl_state_size(&size);
-
- MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
-
- /* store data */
- res = inv_save_mpl_states(calData, size);
- if(res != 0)
- {
- MPL_LOGE("inv_save_mpl_states() failed");
- }
-
- STORECAL_LOG("Exiting inv_store_cal\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Load a calibration file.
- *
- * @pre Must be in INV_STATE_DMP_OPENED state.
- * inv_dmp_open() or inv_dmp_stop() must have been called.
- * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- * been called.
- *
- * @return 0 or error code.
- */
-inv_error_t inv_load_calibration(void)
-{
- unsigned char *calData= NULL;
- inv_error_t result = 0;
- size_t bytesRead = 0;
-
- result = inv_read_cal(&calData, &bytesRead);
- if(result != INV_SUCCESS) {
- MPL_LOGE("Could not load cal file - "
- "aborting\n");
- goto free_mem_n_exit;
- }
-
- result = inv_load_mpl_states(calData, bytesRead);
- if (result != INV_SUCCESS) {
- MPL_LOGE("Could not load the calibration data - "
- "error %d - aborting\n", result);
- goto free_mem_n_exit;
- }
-
-free_mem_n_exit:
- inv_free(calData);
- return result;
-}
-
-/**
- * @brief Store runtime calibration data to a file
- *
- * @pre Must be in INV_STATE_DMP_OPENED state.
- * inv_dmp_open() or inv_dmp_stop() must have been called.
- * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- * been called.
- *
- * @return 0 or error code.
- */
-inv_error_t inv_store_calibration(void)
-{
- unsigned char *calData;
- inv_error_t result;
- size_t length;
-
- result = inv_get_mpl_state_size(&length);
- calData = (unsigned char *)inv_malloc(length);
- if (!calData) {
- MPL_LOGE("Could not allocate buffer of %d bytes - "
- "aborting\n", length);
- return INV_ERROR_MEMORY_EXAUSTED;
- }
- else {
- MPL_LOGI("mpl state size = %d", length);
- }
-
- result = inv_save_mpl_states(calData, length);
- if (result != INV_SUCCESS) {
- MPL_LOGE("Could not save mpl states - "
- "error %d - aborting\n", result);
- goto free_mem_n_exit;
- }
- else {
- MPL_LOGE("calData from inv_save_mpl_states, size=%d",
- strlen((char *)calData));
- }
-
- result = inv_write_cal(calData, length);
- if (result != INV_SUCCESS) {
- MPL_LOGE("Could not store calibrated data on file - "
- "error %d - aborting\n", result);
- goto free_mem_n_exit;
-
- }
-
-free_mem_n_exit:
- inv_free(calData);
- return result;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
deleted file mode 100644
index 115b34c..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*******************************************************************************
- *
- * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $
- *
- ******************************************************************************/
-
-#ifndef INV_MPL_STORED_DATA_H
-#define INV_MPL_STORED_DATA_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
- Includes.
-*/
-#include "mltypes.h"
-
-/*
- Defines
-*/
-#define MLCAL_FILE "/data/inv_cal_data.bin"
-
-/*
- APIs
-*/
-inv_error_t inv_load_calibration(void);
-inv_error_t inv_store_calibration(void);
-
-/*
- Internal APIs
-*/
-inv_error_t inv_read_cal(unsigned char **, size_t *);
-inv_error_t inv_write_cal(unsigned char *cal, size_t len);
-inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len);
-inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len);
-
-/*
- Other prototypes
-*/
-inv_error_t inv_load_cal(unsigned char *calData);
-inv_error_t inv_store_cal(unsigned char *calData, size_t length);
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* INV_MPL_STORED_DATA_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
deleted file mode 100644
index d0c4513..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c
+++ /dev/null
@@ -1,423 +0,0 @@
-#include <string.h>
-#include <stdio.h>
-#include "ml_sysfs_helper.h"
-#include <dirent.h>
-#include <ctype.h>
-#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu"
-
-enum PROC_SYSFS_CMD {
- CMD_GET_SYSFS_PATH,
- CMD_GET_DMP_PATH,
- CMD_GET_CHIP_NAME,
- CMD_GET_SYSFS_KEY,
- CMD_GET_TRIGGER_PATH,
- CMD_GET_DEVICE_NODE
-};
-static char sysfs_path[100];
-static char *chip_name[] = {
- "ITG3500",
- "MPU6050",
- "MPU9150",
- "MPU3050",
- "MPU6500"
-};
-static int chip_ind;
-static int initialized =0;
-static int status = 0;
-static int iio_initialized = 0;
-static int iio_dev_num = 0;
-
-#define IIO_MAX_NAME_LENGTH 30
-
-#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
-#define FORMAT_TYPE_FILE "%s_type"
-
-#define CHIP_NUM ARRAY_SIZE(chip_name)
-
-static const char *iio_dir = "/sys/bus/iio/devices/";
-
-/**
- * find_type_by_name() - function to match top level types by name
- * @name: top level type instance name
- * @type: the type of top level instance being sort
- *
- * Typical types this is used for are device and trigger.
- **/
-int find_type_by_name(const char *name, const char *type)
-{
- const struct dirent *ent;
- int number, numstrlen;
-
- FILE *nameFile;
- DIR *dp;
- char thisname[IIO_MAX_NAME_LENGTH];
- char *filename;
-
- dp = opendir(iio_dir);
- if (dp == NULL) {
- printf("No industrialio devices available");
- return -ENODEV;
- }
-
- while (ent = readdir(dp), ent != NULL) {
- if (strcmp(ent->d_name, ".") != 0 &&
- strcmp(ent->d_name, "..") != 0 &&
- strlen(ent->d_name) > strlen(type) &&
- strncmp(ent->d_name, type, strlen(type)) == 0) {
- numstrlen = sscanf(ent->d_name + strlen(type),
- "%d",
- &number);
- /* verify the next character is not a colon */
- if (strncmp(ent->d_name + strlen(type) + numstrlen,
- ":",
- 1) != 0) {
- filename = malloc(strlen(iio_dir)
- + strlen(type)
- + numstrlen
- + 6);
- if (filename == NULL)
- return -ENOMEM;
- sprintf(filename, "%s%s%d/name",
- iio_dir,
- type,
- number);
- nameFile = fopen(filename, "r");
- if (!nameFile)
- continue;
- free(filename);
- fscanf(nameFile, "%s", thisname);
- if (strcmp(name, thisname) == 0)
- return number;
- fclose(nameFile);
- }
- }
- }
- return -ENODEV;
-}
-
-/* mode 0: search for which chip in the system and fill sysfs path
- mode 1: return event number
- */
-static int parsing_proc_input(int mode, char *name){
- const char input[] = "/proc/bus/input/devices";
- char line[4096], d;
- char tmp[100];
- FILE *fp;
- int i, j, result, find_flag;
- int event_number = -1;
- int input_number = -1;
-
- if(NULL == (fp = fopen(input, "rt")) ){
- return -1;
- }
- result = 1;
- find_flag = 0;
- while(result != 0 && find_flag < 2){
- i = 0;
- d = 0;
- memset(line, 0, 100);
- while(d != '\n'){
- result = fread(&d, 1, 1, fp);
- if(result == 0){
- line[0] = 0;
- break;
- }
- sprintf(&line[i], "%c", d);
- i ++;
- }
- if(line[0] == 'N'){
- i = 1;
- while(line[i] != '"'){
- i++;
- }
- i++;
- j = 0;
- find_flag = 0;
- if (mode == 0){
- while(j < CHIP_NUM){
- if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){
- find_flag = 1;
- chip_ind = j;
- }
- j++;
- }
- } else if (mode != 0){
- if(!memcmp(&line[i], name, strlen(name))){
- find_flag = 1;
- }
- }
- }
- if(find_flag){
- if(mode == 0){
- if(line[0] == 'S'){
- memset(tmp, 0, 100);
- i =1;
- while(line[i] != '=') i++;
- i++;
- j = 0;
- while(line[i] != '\n'){
- tmp[j] = line[i];
- i ++; j++;
- }
- sprintf(sysfs_path, "%s%s", "/sys", tmp);
- find_flag++;
- }
- } else if(mode == 1){
- if(line[0] == 'H') {
- i = 2;
- while(line[i] != '=') i++;
- while(line[i] != 't') i++;
- i++;
- event_number = 0;
- while(line[i] != '\n'){
- if(line[i] >= '0' && line[i] <= '9')
- event_number = event_number*10 + line[i]-0x30;
- i ++;
- }
- find_flag ++;
- }
- } else if (mode == 2) {
- if(line[0] == 'S'){
- memset(tmp, 0, 100);
- i =1;
- while(line[i] != '=') i++;
- i++;
- j = 0;
- while(line[i] != '\n'){
- tmp[j] = line[i];
- i ++; j++;
- }
- input_number = 0;
- if(tmp[j-2] >= '0' && tmp[j-2] <= '9')
- input_number += (tmp[j-2]-0x30)*10;
- if(tmp[j-1] >= '0' && tmp[j-1] <= '9')
- input_number += (tmp[j-1]-0x30);
- find_flag++;
- }
- }
- }
- }
- fclose(fp);
- if(find_flag == 0){
- return -1;
- }
- if(0 == mode)
- status = 1;
- if (mode == 1)
- return event_number;
- if (mode == 2)
- return input_number;
- return 0;
-
-}
-static void init_iio() {
- int i, j;
- char iio_chip[10];
- int dev_num;
- for(j=0; j< CHIP_NUM; j++) {
- for (i=0; i<strlen(chip_name[j]); i++) {
- iio_chip[i] = tolower(chip_name[j][i]);
- }
- iio_chip[strlen(chip_name[0])] = '\0';
- dev_num = find_type_by_name(iio_chip, "iio:device");
- if(dev_num >= 0) {
- iio_initialized = 1;
- iio_dev_num = dev_num;
- chip_ind = j;
- }
- }
-}
-
-static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data)
-{
- char key_path[100];
- FILE *fp;
- int i, result;
- if(initialized == 0){
- parsing_proc_input(0, NULL);
- initialized = 1;
- }
- if(initialized && status == 0) {
- init_iio();
- if (iio_initialized == 0)
- return -1;
- }
-
- memset(key_path, 0, 100);
- switch(cmd){
- case CMD_GET_SYSFS_PATH:
- if (iio_initialized == 1)
- sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num);
- else
- sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu");
- break;
- case CMD_GET_DMP_PATH:
- if (iio_initialized == 1)
- sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num);
- else
- sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware");
- break;
- case CMD_GET_CHIP_NAME:
- sprintf(data, "%s", chip_name[chip_ind]);
- break;
- case CMD_GET_TRIGGER_PATH:
- sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num);
- break;
- case CMD_GET_DEVICE_NODE:
- sprintf(data, "/dev/iio:device%d", iio_dev_num);
- break;
- case CMD_GET_SYSFS_KEY:
- memset(key_path, 0, 100);
- if (iio_initialized == 1)
- sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num);
- else
- sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key");
-
- if((fp = fopen(key_path, "rt")) == NULL)
- return -1;
- for(i=0;i<16;i++){
- fscanf(fp, "%02x", &result);
- data[i] = (char)result;
- }
-
- fclose(fp);
- break;
- default:
- break;
- }
- return 0;
-}
-/**
- * @brief return sysfs key. if the key is not available
- * return false. So the return value must be checked
- * to make sure the path is valid.
- * @unsigned char *name: This should be array big enough to hold the key
- * It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- */
-inv_error_t inv_get_sysfs_key(unsigned char *key)
-{
- if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
-
-/**
- * @brief return the sysfs path. If the path is not
- * found yet. return false. So the return value must be checked
- * to make sure the path is valid.
- * @unsigned char *name: This should be array big enough to hold the sysfs
- * path. It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- */
-inv_error_t inv_get_sysfs_path(char *name)
-{
- if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
-
-inv_error_t inv_get_sysfs_abs_path(char *name)
-{
- strcpy(name, MPU_SYSFS_ABS_PATH);
- return INV_SUCCESS;
-}
-
-/**
- * @brief return the dmp file path. If the path is not
- * found yet. return false. So the return value must be checked
- * to make sure the path is valid.
- * @unsigned char *name: This should be array big enough to hold the dmp file
- * path. It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- */
-inv_error_t inv_get_dmpfile(char *name)
-{
- if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
-/**
- * @brief return the chip name. If the chip is not
- * found yet. return false. So the return value must be checked
- * to make sure the path is valid.
- * @unsigned char *name: This should be array big enough to hold the chip name
- * path(8 bytes). It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- */
-inv_error_t inv_get_chip_name(char *name)
-{
- if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
-/**
- * @brief return event handler number. If the handler number is not found
- * return false. the return value must be checked
- * to make sure the path is valid.
- * @unsigned char *name: This should be array big enough to hold the chip name
- * path(8 bytes). It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- * @int *num: event number store
- */
-inv_error_t inv_get_handler_number(const char *name, int *num)
-{
- initialized = 0;
- if ((*num = parsing_proc_input(1, (char *)name)) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
-
-/**
- * @brief return input number. If the handler number is not found
- * return false. the return value must be checked
- * to make sure the path is valid.
- * @unsigned char *name: This should be array big enough to hold the chip name
- * path(8 bytes). It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- * @int *num: input number store
- */
-inv_error_t inv_get_input_number(const char *name, int *num)
-{
- initialized = 0;
- if ((*num = parsing_proc_input(2, (char *)name)) < 0)
- return INV_ERROR_NOT_OPENED;
- else {
- return INV_SUCCESS;
- }
-}
-
-/**
- * @brief return iio trigger name. If iio is not initialized, return false.
- * So the return must be checked to make sure the numeber is valid.
- * @unsigned char *name: This should be array big enough to hold the trigger
- * name. It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- */
-inv_error_t inv_get_iio_trigger_path(char *name)
-{
- if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
-
-/**
- * @brief return iio device node. If iio is not initialized, return false.
- * So the return must be checked to make sure the numeber is valid.
- * @unsigned char *name: This should be array big enough to hold the device
- * node. It should be zeroed before calling this function.
- * Or it could have unpredicable result.
- */
-inv_error_t inv_get_iio_device_node(char *name)
-{
- if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0)
- return INV_ERROR_NOT_OPENED;
- else
- return INV_SUCCESS;
-}
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
deleted file mode 100644
index 9470874..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*******************************************************************************
- *
- * $Id$
- *
- ******************************************************************************/
-
-#ifndef MLDMP_SYSFS_HELPER_H__
-#define MLDMP_SYSFS_HELPER_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "invensense.h"
-
-int find_type_by_name(const char *name, const char *type);
-inv_error_t inv_get_sysfs_path(char *name);
-inv_error_t inv_get_sysfs_abs_path(char *name);
-inv_error_t inv_get_dmpfile(char *name);
-inv_error_t inv_get_chip_name(char *name);
-inv_error_t inv_get_sysfs_key(unsigned char *key);
-inv_error_t inv_get_handler_number(const char *name, int *num);
-inv_error_t inv_get_input_number(const char *name, int *num);
-inv_error_t inv_get_iio_trigger_path(char *name);
-inv_error_t inv_get_iio_device_node(char *name);
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* MLDMP_SYSFS_HELPER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
deleted file mode 100644
index d4f8912..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- $License:
- Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-#ifndef _MLOS_H
-#define _MLOS_H
-
-#ifndef __KERNEL__
-#include <stdio.h>
-#endif
-#include <pthread.h>
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#if defined(LINUX) || defined(__KERNEL__)
-typedef pthread_mutex_t* HANDLE;
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- /* - MLOSCreateFile defines. - */
-
-#define MLOS_GENERIC_READ ((unsigned int)0x80000000)
-#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000)
-#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001)
-#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002)
-#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003)
-
- /* ---------- */
- /* - Enums. - */
- /* ---------- */
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-
-#ifndef __KERNEL__
-#include <string.h>
- void *inv_malloc(unsigned int numBytes);
- inv_error_t inv_free(void *ptr);
- inv_error_t inv_create_mutex(HANDLE *mutex);
- inv_error_t inv_lock_mutex(HANDLE mutex);
- inv_error_t inv_unlock_mutex(HANDLE mutex);
- FILE *inv_fopen(char *filename);
- void inv_fclose(FILE *fp);
-
- inv_error_t inv_destroy_mutex(HANDLE handle);
-
- void inv_sleep(int mSecs);
- unsigned long inv_get_tick_count(void);
-
- /* Kernel implmentations */
-#define GFP_KERNEL (0x70)
- static inline void *kmalloc(size_t size,
- unsigned int gfp_flags)
- {
- (void)gfp_flags;
- return inv_malloc((unsigned int)size);
- }
- static inline void *kzalloc(size_t size, unsigned int gfp_flags)
- {
- void *tmp = inv_malloc((unsigned int)size);
- (void)gfp_flags;
- if (tmp)
- memset(tmp, 0, size);
- return tmp;
- }
- static inline void kfree(void *ptr)
- {
- inv_free(ptr);
- }
- static inline void msleep(long msecs)
- {
- inv_sleep(msecs);
- }
- static inline void udelay(unsigned long usecs)
- {
- inv_sleep((usecs + 999) / 1000);
- }
-#else
-#include <linux/delay.h>
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* _MLOS_H */
diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
deleted file mode 100644
index 5424508..0000000
--- a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c
+++ /dev/null
@@ -1,190 +0,0 @@
-/*
- $License:
- Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*******************************************************************************
- *
- * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- ******************************************************************************/
-
-/**
- * @defgroup MLOS
- * @brief OS Interface.
- *
- * @{
- * @file mlos.c
- * @brief OS Interface.
- */
-
-/* ------------- */
-/* - Includes. - */
-/* ------------- */
-
-#include <sys/time.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdlib.h>
-#include <errno.h>
-
-#include "stdint_invensense.h"
-#include "mlos.h"
-
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/**
- * @brief Allocate space
- * @param num_bytes number of bytes
- * @return pointer to allocated space
- */
-void *inv_malloc(unsigned int num_bytes)
-{
- // Allocate space.
- void *alloc_ptr = malloc(num_bytes);
- return alloc_ptr;
-}
-
-
-/**
- * @brief Free allocated space
- * @param ptr pointer to space to deallocate
- * @return error code.
- */
-inv_error_t inv_free(void *ptr)
-{
- if (ptr)
- free(ptr);
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief Mutex create function
- * @param mutex pointer to mutex handle
- * @return error code.
- */
-inv_error_t inv_create_mutex(HANDLE *mutex)
-{
- int res;
- pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t));
- if(pm == NULL)
- return INV_ERROR;
-
- res = pthread_mutex_init(pm, NULL);
- if(res == -1) {
- free(pm);
- return INV_ERROR_OS_CREATE_FAILED;
- }
-
- *mutex = (HANDLE)pm;
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief Mutex lock function
- * @param mutex Mutex handle
- * @return error code.
- */
-inv_error_t inv_lock_mutex(HANDLE mutex)
-{
- int res;
- pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
-
- res = pthread_mutex_lock(pm);
- if(res == -1)
- return INV_ERROR_OS_LOCK_FAILED;
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief Mutex unlock function
- * @param mutex mutex handle
- * @return error code.
- */
-inv_error_t inv_unlock_mutex(HANDLE mutex)
-{
- int res;
- pthread_mutex_t *pm = (pthread_mutex_t *)mutex;
-
- res = pthread_mutex_unlock(pm);
- if(res == -1)
- return INV_ERROR_OS_LOCK_FAILED;
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief open file
- * @param filename name of the file to open.
- * @return error code.
- */
-FILE *inv_fopen(char *filename)
-{
- FILE *fp = fopen(filename, "r");
- return fp;
-}
-
-
-/**
- * @brief close the file.
- * @param fp handle to file to close.
- * @return error code.
- */
-void inv_fclose(FILE *fp)
-{
- fclose(fp);
-}
-
-/**
- * @brief Close Handle
- * @param handle handle to the resource.
- * @return Zero if success, an error code otherwise.
- */
-inv_error_t inv_destroy_mutex(HANDLE handle)
-{
- int error;
- pthread_mutex_t *pm = (pthread_mutex_t *)handle;
- error = pthread_mutex_destroy(pm);
- if (error)
- return errno;
- free((void*) handle);
-
- return INV_SUCCESS;}
-
-
-/**
- * @brief Sleep function.
- */
-void inv_sleep(int m_secs)
-{
- usleep(m_secs * 1000);
-}
-
-
-/**
- * @brief get system's internal tick count.
- * Used for time reference.
- * @return current tick count.
- */
-unsigned long inv_get_tick_count()
-{
- struct timeval tv;
-
- if (gettimeofday(&tv, NULL) != 0)
- return 0;
-
- return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
-}
-
-/** @} */
-
diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c
deleted file mode 100644
index 8317957..0000000
--- a/60xx/libsensors_iio/software/core/mllite/message_layer.c
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup Message_Layer message_layer
- * @brief Motion Library - Message Layer
- * Holds Low Occurance messages
- *
- * @{
- * @file message_layer.c
- * @brief Holds Low Occurance Messages.
- */
-#include "message_layer.h"
-#include "log.h"
-
-struct message_holder_t {
- long message;
-};
-
-static struct message_holder_t mh;
-
-/** Sets a message.
-* @param[in] set The flags to set.
-* @param[in] clear Before setting anything this will clear these messages,
-* which is useful for mutually exclusive messages such
-* a motion or no motion message.
-* @param[in] level Level of the messages. It starts at 0, and may increase
-* in the future to allow more messages if the bit storage runs out.
-*/
-void inv_set_message(long set, long clear, int level)
-{
- if (level == 0) {
- mh.message &= ~clear;
- mh.message |= set;
- }
-}
-
-/** Returns Message Flags for Level 0 Messages.
-* Levels are to allow expansion of more messages in the future.
-* @param[in] clear If set, will clear the message. Typically this will be set
-* for one reader, so that you don't get the same message over and over.
-* @return bit field to corresponding message.
-*/
-long inv_get_message_level_0(int clear)
-{
- long msg;
- msg = mh.message;
- if (clear) {
- mh.message = 0;
- }
- return msg;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h
deleted file mode 100644
index df0c0e2..0000000
--- a/60xx/libsensors_iio/software/core/mllite/message_layer.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_MESSAGE_LAYER_H__
-#define INV_MESSAGE_LAYER_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* Level 0 Type Messages */
- /** A motion event has occured */
-#define INV_MSG_MOTION_EVENT (0x01)
- /** A no motion event has occured */
-#define INV_MSG_NO_MOTION_EVENT (0x02)
- /** A setting of the gyro bias has occured */
-#define INV_MSG_NEW_GB_EVENT (0x04)
- /** A setting of the compass bias has occured */
-#define INV_MSG_NEW_CB_EVENT (0x08)
- /** A setting of the accel bias has occured */
-#define INV_MSG_NEW_AB_EVENT (0x10)
-
- void inv_set_message(long set, long clear, int level);
- long inv_get_message_level_0(int clear);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MESSAGE_LAYER_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.c b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c
deleted file mode 100644
index c30d693..0000000
--- a/60xx/libsensors_iio/software/core/mllite/ml_math_func.c
+++ /dev/null
@@ -1,706 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-/**
- * @defgroup ML_MATH_FUNC ml_math_func
- * @brief Motion Library - Math Functions
- * Common math functions the Motion Library
- *
- * @{
- * @file ml_math_func.c
- * @brief Math Functions.
- */
-
-#include "mlmath.h"
-#include "ml_math_func.h"
-#include "mlinclude.h"
-#include <string.h>
-
-/** @internal
- * Does the cross product of compass by gravity, then converts that
- * to the world frame using the quaternion, then computes the angle that
- * is made.
- *
- * @param[in] compass Compass Vector (Body Frame), length 3
- * @param[in] grav Gravity Vector (Body Frame), length 3
- * @param[in] quat Quaternion, Length 4
- * @return Angle Cross Product makes after quaternion rotation.
- */
-float inv_compass_angle(const long *compass, const long *grav, const float *quat)
-{
- float cgcross[4], q1[4], q2[4], qi[4];
- float angW;
-
- // Compass cross Gravity
- cgcross[0] = 0.f;
- cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
- cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
- cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
-
- // Now convert cross product into world frame
- inv_q_multf(quat, cgcross, q1);
- inv_q_invertf(quat, qi);
- inv_q_multf(q1, qi, q2);
-
- // Protect against atan2 of 0,0
- if ((q2[2] == 0.f) && (q2[1] == 0.f))
- return 0.f;
-
- // This is the unfiltered heading correction
- angW = -atan2f(q2[2], q2[1]);
- return angW;
-}
-
-/**
- * @brief The gyro data magnitude squared :
- * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
- * @param[in] gyro Gyro data scaled with 1 dps = 2^16
- * @return the computed magnitude squared output of the gyroscope.
- */
-unsigned long inv_get_gyro_sum_of_sqr(const long *gyro)
-{
- unsigned long gmag = 0;
- long temp;
- int kk;
-
- for (kk = 0; kk < 3; ++kk) {
- temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2));
- gmag += temp * temp;
- }
-
- return gmag;
-}
-
-/** Performs a multiply and shift by 29. These are good functions to write in assembly on
- * with devices with small memory where you want to get rid of the long long which some
- * assemblers don't handle well
- * @param[in] a
- * @param[in] b
- * @return ((long long)a*b)>>29
-*/
-long inv_q29_mult(long a, long b)
-{
-#ifdef UMPL_ELIMINATE_64BIT
- long result;
- result = (long)((float)a * b / (1L << 29));
- return result;
-#else
- long long temp;
- long result;
- temp = (long long)a * b;
- result = (long)(temp >> 29);
- return result;
-#endif
-}
-
-/** Performs a multiply and shift by 30. These are good functions to write in assembly on
- * with devices with small memory where you want to get rid of the long long which some
- * assemblers don't handle well
- * @param[in] a
- * @param[in] b
- * @return ((long long)a*b)>>30
-*/
-long inv_q30_mult(long a, long b)
-{
-#ifdef UMPL_ELIMINATE_64BIT
- long result;
- result = (long)((float)a * b / (1L << 30));
- return result;
-#else
- long long temp;
- long result;
- temp = (long long)a * b;
- result = (long)(temp >> 30);
- return result;
-#endif
-}
-
-#ifndef UMPL_ELIMINATE_64BIT
-long inv_q30_div(long a, long b)
-{
- long long temp;
- long result;
- temp = (((long long)a) << 30) / b;
- result = (long)temp;
- return result;
-}
-#endif
-
-/** Performs a multiply and shift by shift. These are good functions to write
- * in assembly on with devices with small memory where you want to get rid of
- * the long long which some assemblers don't handle well
- * @param[in] a First multicand
- * @param[in] b Second multicand
- * @param[in] shift Shift amount after multiplying
- * @return ((long long)a*b)<<shift
-*/
-#ifndef UMPL_ELIMINATE_64BIT
-long inv_q_shift_mult(long a, long b, int shift)
-{
- long result;
- result = (long)(((long long)a * b) >> shift);
- return result;
-}
-#endif
-
-/** Performs a fixed point quaternion multiply.
-* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled
-* to 2^30
-* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled
-* to 2^30
-* @param[out] qProd Product after quaternion multiply. Length 4.
-* 1.0 scaled to 2^30.
-*/
-void inv_q_mult(const long *q1, const long *q2, long *qProd)
-{
- INVENSENSE_FUNC_START;
- qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) -
- inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]);
-
- qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) +
- inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]);
-
- qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) +
- inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]);
-
- qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) -
- inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]);
-}
-
-/** Performs a fixed point quaternion addition.
-* @param[in] q1 First Quaternion term, length 4. 1.0 scaled
-* to 2^30
-* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled
-* to 2^30
-* @param[out] qSum Sum after quaternion summation. Length 4.
-* 1.0 scaled to 2^30.
-*/
-void inv_q_add(long *q1, long *q2, long *qSum)
-{
- INVENSENSE_FUNC_START;
- qSum[0] = q1[0] + q2[0];
- qSum[1] = q1[1] + q2[1];
- qSum[2] = q1[2] + q2[2];
- qSum[3] = q1[3] + q2[3];
-}
-
-void inv_vector_normalize(long *vec, int length)
-{
- INVENSENSE_FUNC_START;
- double normSF = 0;
- int ii;
- for (ii = 0; ii < length; ii++) {
- normSF +=
- inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]);
- }
- if (normSF > 0) {
- normSF = 1 / sqrt(normSF);
- for (ii = 0; ii < length; ii++) {
- vec[ii] = (int)((double)vec[ii] * normSF);
- }
- } else {
- vec[0] = 1073741824L;
- for (ii = 1; ii < length; ii++) {
- vec[ii] = 0;
- }
- }
-}
-
-void inv_q_normalize(long *q)
-{
- INVENSENSE_FUNC_START;
- inv_vector_normalize(q, 4);
-}
-
-void inv_q_invert(const long *q, long *qInverted)
-{
- INVENSENSE_FUNC_START;
- qInverted[0] = q[0];
- qInverted[1] = -q[1];
- qInverted[2] = -q[2];
- qInverted[3] = -q[3];
-}
-
-double quaternion_to_rotation_angle(const long *quat) {
- double quat0 = (double )quat[0] / 1073741824;
- if (quat0 > 1.0f) {
- quat0 = 1.0;
- } else if (quat0 < -1.0f) {
- quat0 = -1.0;
- }
-
- return acos(quat0)*2*180/M_PI;
-}
-
-/** Rotates a 3-element vector by Rotation defined by Q
-*/
-void inv_q_rotate(const long *q, const long *in, long *out)
-{
- long q_temp1[4], q_temp2[4];
- long in4[4], out4[4];
-
- // Fixme optimize
- in4[0] = 0;
- memcpy(&in4[1], in, 3 * sizeof(long));
- inv_q_mult(q, in4, q_temp1);
- inv_q_invert(q, q_temp2);
- inv_q_mult(q_temp1, q_temp2, out4);
- memcpy(out, &out4[1], 3 * sizeof(long));
-}
-
-void inv_q_multf(const float *q1, const float *q2, float *qProd)
-{
- INVENSENSE_FUNC_START;
- qProd[0] =
- (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
- qProd[1] =
- (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
- qProd[2] =
- (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
- qProd[3] =
- (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
-}
-
-void inv_q_addf(const float *q1, const float *q2, float *qSum)
-{
- INVENSENSE_FUNC_START;
- qSum[0] = q1[0] + q2[0];
- qSum[1] = q1[1] + q2[1];
- qSum[2] = q1[2] + q2[2];
- qSum[3] = q1[3] + q2[3];
-}
-
-void inv_q_normalizef(float *q)
-{
- INVENSENSE_FUNC_START;
- float normSF = 0;
- float xHalf = 0;
- normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
- if (normSF < 2) {
- xHalf = 0.5f * normSF;
- normSF = normSF * (1.5f - xHalf * normSF * normSF);
- normSF = normSF * (1.5f - xHalf * normSF * normSF);
- normSF = normSF * (1.5f - xHalf * normSF * normSF);
- normSF = normSF * (1.5f - xHalf * normSF * normSF);
- q[0] *= normSF;
- q[1] *= normSF;
- q[2] *= normSF;
- q[3] *= normSF;
- } else {
- q[0] = 1.0;
- q[1] = 0.0;
- q[2] = 0.0;
- q[3] = 0.0;
- }
- normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
-}
-
-/** Performs a length 4 vector normalization with a square root.
-* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero.
-*/
-void inv_q_norm4(float *q)
-{
- float mag;
- mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
- if (mag) {
- q[0] /= mag;
- q[1] /= mag;
- q[2] /= mag;
- q[3] /= mag;
- } else {
- q[0] = 1.f;
- q[1] = 0.f;
- q[2] = 0.f;
- q[3] = 0.f;
- }
-}
-
-void inv_q_invertf(const float *q, float *qInverted)
-{
- INVENSENSE_FUNC_START;
- qInverted[0] = q[0];
- qInverted[1] = -q[1];
- qInverted[2] = -q[2];
- qInverted[3] = -q[3];
-}
-
-/**
- * Converts a quaternion to a rotation matrix.
- * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
- * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
- * First 3 elements of the rotation matrix, represent
- * the first row of the matrix. Rotation matrix multiplied
- * by a 3 element column vector transform a vector from Body
- * to World.
- */
-void inv_quaternion_to_rotation(const long *quat, long *rot)
-{
- rot[0] =
- inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
- quat[0]) -
- 1073741824L;
- rot[1] =
- inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
- rot[2] =
- inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
- rot[3] =
- inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
- rot[4] =
- inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
- quat[0]) -
- 1073741824L;
- rot[5] =
- inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
- rot[6] =
- inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
- rot[7] =
- inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
- rot[8] =
- inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
- quat[0]) -
- 1073741824L;
-}
-
-/**
- * Converts a quaternion to a rotation vector. A rotation vector is
- * a method to represent a 4-element quaternion vector in 3-elements.
- * To get the quaternion from the 3-elements, The last 3-elements of
- * the quaternion will be the given rotation vector. The first element
- * of the quaternion will be the positive value that will be required
- * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units.
- * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
- * @param[out] rot Rotation vector in fixed point. One is 2^30.
- */
-void inv_quaternion_to_rotation_vector(const long *quat, long *rot)
-{
- rot[0] = quat[1];
- rot[1] = quat[2];
- rot[2] = quat[3];
-
- if (quat[0] < 0.0) {
- rot[0] = -rot[0];
- rot[1] = -rot[1];
- rot[2] = -rot[2];
- }
-}
-
-/** Converts a 32-bit long to a big endian byte stream */
-unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
-{
- big8[0] = (unsigned char)((x >> 24) & 0xff);
- big8[1] = (unsigned char)((x >> 16) & 0xff);
- big8[2] = (unsigned char)((x >> 8) & 0xff);
- big8[3] = (unsigned char)(x & 0xff);
- return big8;
-}
-
-/** Converts a big endian byte stream into a 32-bit long */
-long inv_big8_to_int32(const unsigned char *big8)
-{
- long x;
- x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8)
- | ((long)big8[3]);
- return x;
-}
-
-/** Converts a big endian byte stream into a 16-bit integer (short) */
-short inv_big8_to_int16(const unsigned char *big8)
-{
- short x;
- x = ((short)big8[0] << 8) | ((short)big8[1]);
- return x;
-}
-
-/** Converts a little endian byte stream into a 16-bit integer (short) */
-short inv_little8_to_int16(const unsigned char *little8)
-{
- short x;
- x = ((short)little8[1] << 8) | ((short)little8[0]);
- return x;
-}
-
-/** Converts a 16-bit short to a big endian byte stream */
-unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
-{
- big8[0] = (unsigned char)((x >> 8) & 0xff);
- big8[1] = (unsigned char)(x & 0xff);
- return big8;
-}
-
-void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
-{
- int k, l, i, j;
- for (i = 0, k = 0; i < *n; i++, k++) {
- for (j = 0, l = 0; j < *n; j++, l++) {
- if (i == x)
- i++;
- if (j == y)
- j++;
- *(b + 6 * k + l) = *(a + 6 * i + j);
- }
- }
- *n = *n - 1;
-}
-
-void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
-{
- int k, l, i, j;
- for (i = 0, k = 0; i < *n; i++, k++) {
- for (j = 0, l = 0; j < *n; j++, l++) {
- if (i == x)
- i++;
- if (j == y)
- j++;
- *(b + 6 * k + l) = *(a + 6 * i + j);
- }
- }
- *n = *n - 1;
-}
-
-float inv_matrix_det(float *p, int *n)
-{
- float d[6][6], sum = 0;
- int i, j, m;
- m = *n;
- if (*n == 2)
- return (*p ** (p + 7) - *(p + 1) ** (p + 6));
- for (i = 0, j = 0; j < m; j++) {
- *n = m;
- inv_matrix_det_inc(p, &d[0][0], n, i, j);
- sum =
- sum + *(p + 6 * i + j) * SIGNM(i +
- j) *
- inv_matrix_det(&d[0][0], n);
- }
-
- return (sum);
-}
-
-double inv_matrix_detd(double *p, int *n)
-{
- double d[6][6], sum = 0;
- int i, j, m;
- m = *n;
- if (*n == 2)
- return (*p ** (p + 7) - *(p + 1) ** (p + 6));
- for (i = 0, j = 0; j < m; j++) {
- *n = m;
- inv_matrix_det_incd(p, &d[0][0], n, i, j);
- sum =
- sum + *(p + 6 * i + j) * SIGNM(i +
- j) *
- inv_matrix_detd(&d[0][0], n);
- }
-
- return (sum);
-}
-
-/** Wraps angle from (-M_PI,M_PI]
- * @param[in] ang Angle in radians to wrap
- * @return Wrapped angle from (-M_PI,M_PI]
- */
-float inv_wrap_angle(float ang)
-{
- if (ang > M_PI)
- return ang - 2 * (float)M_PI;
- else if (ang <= -(float)M_PI)
- return ang + 2 * (float)M_PI;
- else
- return ang;
-}
-
-/** Finds the minimum angle difference ang1-ang2 such that difference
- * is between [-M_PI,M_PI]
- * @param[in] ang1
- * @param[in] ang2
- * @return angle difference ang1-ang2
- */
-float inv_angle_diff(float ang1, float ang2)
-{
- float d;
- ang1 = inv_wrap_angle(ang1);
- ang2 = inv_wrap_angle(ang2);
- d = ang1 - ang2;
- if (d > M_PI)
- d -= 2 * (float)M_PI;
- else if (d < -(float)M_PI)
- d += 2 * (float)M_PI;
- return d;
-}
-
-/** bernstein hash, derived from public domain source */
-uint32_t inv_checksum(const unsigned char *str, int len)
-{
- uint32_t hash = 5381;
- int i, c;
-
- for (i = 0; i < len; i++) {
- c = *(str + i);
- hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
- }
-
- return hash;
-}
-
-static unsigned short inv_row_2_scale(const signed char *row)
-{
- unsigned short b;
-
- if (row[0] > 0)
- b = 0;
- else if (row[0] < 0)
- b = 4;
- else if (row[1] > 0)
- b = 1;
- else if (row[1] < 0)
- b = 5;
- else if (row[2] > 0)
- b = 2;
- else if (row[2] < 0)
- b = 6;
- else
- b = 7; // error
- return b;
-}
-
-
-/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
-* @param[in] mtx Orientation matrix to convert to a scalar.
-* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
-* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
-* the column the one is on for the second row with bit number 5 being the sign.
-* The next 2 bits (6 and 7) represent the column the one is on for the third row with
-* bit number 8 being the sign. In binary the identity matrix would therefor be:
-* 010_001_000 or 0x88 in hex.
-*/
-unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
-{
-
- unsigned short scalar;
-
- /*
- XYZ 010_001_000 Identity Matrix
- XZY 001_010_000
- YXZ 010_000_001
- YZX 000_010_001
- ZXY 001_000_010
- ZYX 000_001_010
- */
-
- scalar = inv_row_2_scale(mtx);
- scalar |= inv_row_2_scale(mtx + 3) << 3;
- scalar |= inv_row_2_scale(mtx + 6) << 6;
-
-
- return scalar;
-}
-
-/** Uses the scalar orientation value to convert from chip frame to body frame
-* @param[in] orientation A scalar that represent how to go from chip to body frame
-* @param[in] input Input vector, length 3
-* @param[out] output Output vector, length 3
-*/
-void inv_convert_to_body(unsigned short orientation, const long *input, long *output)
-{
- output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004);
- output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020);
- output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100);
-}
-
-/** Uses the scalar orientation value to convert from body frame to chip frame
-* @param[in] orientation A scalar that represent how to go from chip to body frame
-* @param[in] input Input vector, length 3
-* @param[out] output Output vector, length 3
-*/
-void inv_convert_to_chip(unsigned short orientation, const long *input, long *output)
-{
- output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004);
- output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020);
- output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100);
-}
-
-
-/** Uses the scalar orientation value to convert from chip frame to body frame and
-* apply appropriate scaling.
-* @param[in] orientation A scalar that represent how to go from chip to body frame
-* @param[in] sensitivity Sensitivity scale
-* @param[in] input Input vector, length 3
-* @param[out] output Output vector, length 3
-*/
-void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output)
-{
- output[0] = inv_q30_mult(input[orientation & 0x03] *
- SIGNSET(orientation & 0x004), sensitivity);
- output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] *
- SIGNSET(orientation & 0x020), sensitivity);
- output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] *
- SIGNSET(orientation & 0x100), sensitivity);
-}
-
-/** find a norm for a vector
-* @param[in] x a vector [3x1]
-* @return the normalize vector.
-*/
-double inv_vector_norm(const float *x)
-{
- return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
-}
-
-void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
- int i;
- // initial state to zero
- pFilter->state[0] = 0;
- pFilter->state[1] = 0;
-
- // set up coefficients
- for (i=0; i<5; i++) {
- pFilter->c[i] = pBiquadCoeff[i];
- }
-}
-
-void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
-{
- pFilter->input = input;
- pFilter->output = input;
- pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
- pFilter->state[1] = pFilter->state[0];
-}
-
-float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) {
- float stateZero;
-
- pFilter->input = input;
- // calculate the new state;
- stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0]
- - pFilter->c[3]*pFilter->state[1];
-
- pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0]
- + pFilter->c[1]*pFilter->state[1];
-
- // update the output and state
- pFilter->output = pFilter->output * pFilter->c[4];
- pFilter->state[1] = pFilter->state[0];
- pFilter->state[0] = stateZero;
- return pFilter->output;
-}
-
-void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) {
-
- cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
- cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
- cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h
deleted file mode 100644
index 535d849..0000000
--- a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INVENSENSE_INV_MATH_FUNC_H__
-#define INVENSENSE_INV_MATH_FUNC_H__
-
-#include "mltypes.h"
-
-#define GYRO_MAG_SQR_SHIFT 6
-#define NUM_ROTATION_MATRIX_ELEMENTS (9)
-#define ROT_MATRIX_SCALE_LONG (1073741824L)
-#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f)
-#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \
- ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT ))
-#define SIGNM(k)((int)(k)&1?-1:1)
-#define SIGNSET(x) ((x) ? -1 : +1)
-
-#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- typedef struct {
- float state[4];
- float c[5];
- float input;
- float output;
- } inv_biquad_filter_t;
-
- static inline float inv_q30_to_float(long q30)
- {
- return (float) q30 / ((float)(1L << 30));
- }
-
- static inline double inv_q30_to_double(long q30)
- {
- return (double) q30 / ((double)(1L << 30));
- }
-
- static inline float inv_q16_to_float(long q16)
- {
- return (float) q16 / (1L << 16);
- }
-
- static inline double inv_q16_to_double(long q16)
- {
- return (double) q16 / (1L << 16);
- }
-
-
-
-
- long inv_q29_mult(long a, long b);
- long inv_q30_mult(long a, long b);
-
- /* UMPL_ELIMINATE_64BIT Notes:
- * An alternate implementation using float instead of long long accudoublemulators
- * is provided for q29_mult and q30_mult.
- * When long long accumulators are used and an alternate implementation is not
- * available, we eliminate the entire function and header with a macro.
- */
-#ifndef UMPL_ELIMINATE_64BIT
- long inv_q30_div(long a, long b);
- long inv_q_shift_mult(long a, long b, int shift);
-#endif
-
- void inv_q_mult(const long *q1, const long *q2, long *qProd);
- void inv_q_add(long *q1, long *q2, long *qSum);
- void inv_q_normalize(long *q);
- void inv_q_invert(const long *q, long *qInverted);
- void inv_q_multf(const float *q1, const float *q2, float *qProd);
- void inv_q_addf(const float *q1, const float *q2, float *qSum);
- void inv_q_normalizef(float *q);
- void inv_q_norm4(float *q);
- void inv_q_invertf(const float *q, float *qInverted);
- void inv_quaternion_to_rotation(const long *quat, long *rot);
- unsigned char *inv_int32_to_big8(long x, unsigned char *big8);
- long inv_big8_to_int32(const unsigned char *big8);
- short inv_big8_to_int16(const unsigned char *big8);
- short inv_little8_to_int16(const unsigned char *little8);
- unsigned char *inv_int16_to_big8(short x, unsigned char *big8);
- float inv_matrix_det(float *p, int *n);
- void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y);
- double inv_matrix_detd(double *p, int *n);
- void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y);
- float inv_wrap_angle(float ang);
- float inv_angle_diff(float ang1, float ang2);
- void inv_quaternion_to_rotation_vector(const long *quat, long *rot);
- unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
- void inv_convert_to_body(unsigned short orientation, const long *input, long *output);
- void inv_convert_to_chip(unsigned short orientation, const long *input, long *output);
- void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output);
- void inv_q_rotate(const long *q, const long *in, long *out);
- void inv_vector_normalize(long *vec, int length);
- uint32_t inv_checksum(const unsigned char *str, int len);
- float inv_compass_angle(const long *compass, const long *grav,
- const float *quat);
- unsigned long inv_get_gyro_sum_of_sqr(const long *gyro);
-
- static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2)
- {
- return (long)((t1 - t2) / 1000000L);
- }
-
- double quaternion_to_rotation_angle(const long *quat);
- double inv_vector_norm(const float *x);
-
- void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff);
- float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input);
- void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input);
- void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.c b/60xx/libsensors_iio/software/core/mllite/mpl.c
deleted file mode 100644
index 9141cc6..0000000
--- a/60xx/libsensors_iio/software/core/mllite/mpl.c
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup MPL mpl
- * @brief Motion Library - Start Point
- * Initializes MPL.
- *
- * @{
- * @file mpl.c
- * @brief MPL start point.
- */
-
-#include "storage_manager.h"
-#include "log.h"
-#include "mpl.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "results_holder.h"
-#include "mlinclude.h"
-
-/**
- * @brief Initializes the MPL. Should be called first and once
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_init_mpl(void)
-{
- inv_init_storage_manager();
-
- /* initialize the start callback manager */
- INV_ERROR_CHECK(inv_init_start_manager());
-
- /* initialize the data builder */
- INV_ERROR_CHECK(inv_init_data_builder());
-
- INV_ERROR_CHECK(inv_enable_results_holder());
-
- return INV_SUCCESS;
-}
-
-const char ml_ver[] = "InvenSense MPL 5.1.2 beta RC9";
-
-/**
- * @brief used to get the MPL version.
- * @param version a string where the MPL version gets stored.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_get_version(char **version)
-{
- INVENSENSE_FUNC_START;
- /* cast out the const */
- *version = (char *)&ml_ver;
- return INV_SUCCESS;
-}
-
-/**
- * @brief Starts the MPL. Typically called after inv_init_mpl() or after a
- * inv_stop_mpl() to start the MPL back up an running.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_start_mpl(void)
-{
- INV_ERROR_CHECK(inv_execute_mpl_start_notification());
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.h b/60xx/libsensors_iio/software/core/mllite/mpl.h
deleted file mode 100644
index a6b5ac7..0000000
--- a/60xx/libsensors_iio/software/core/mllite/mpl.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#include "mltypes.h"
-
-#ifndef INV_MPL_H__
-#define INV_MPL_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_init_mpl(void);
-inv_error_t inv_start_mpl(void);
-inv_error_t inv_get_version(char **version);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MPL_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.c b/60xx/libsensors_iio/software/core/mllite/results_holder.c
deleted file mode 100644
index df58f40..0000000
--- a/60xx/libsensors_iio/software/core/mllite/results_holder.c
+++ /dev/null
@@ -1,522 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup Results_Holder results_holder
- * @brief Motion Library - Results Holder
- * Holds the data for MPL
- *
- * @{
- * @file results_holder.c
- * @brief Results Holder for HAL.
- */
-
-#include <string.h>
-
-#include "results_holder.h"
-#include "ml_math_func.h"
-#include "mlmath.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "message_layer.h"
-#include "log.h"
-
-// These 2 status bits are used to control when the 9 axis quaternion is updated
-#define INV_COMPASS_CORRECTION_SET 1
-#define INV_6_AXIS_QUAT_SET 2
-
-struct results_t {
- long nav_quat[4];
- long gam_quat[4];
- inv_time_t nav_timestamp;
- inv_time_t gam_timestamp;
- long local_field[3]; /**< local earth's magnetic field */
- long mag_scale[3]; /**< scale factor to apply to magnetic field reading */
- long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */
- int acc_state; /**< Describes accel state */
- int got_accel_bias; /**< Flag describing if accel bias is known */
- long compass_bias_error[3]; /**< Error Squared */
- unsigned char motion_state;
- unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */
- long compass_count; /**< compass state internal counter */
- int got_compass_bias; /**< Flag describing if compass bias is known */
- int large_mag_field; /**< Flag describing if there is a large magnetic field */
- int compass_state; /**< Internal compass state */
- long status;
- struct inv_sensor_cal_t *sensor;
- float quat_confidence_interval;
-};
-static struct results_t rh;
-
-/** @internal
-* Store a quaternion more suitable for gaming. This quaternion is often determined
-* using only gyro and accel.
-* @param[in] quat Length 4, Quaternion scaled by 2^30
-*/
-void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp)
-{
- rh.status |= INV_6_AXIS_QUAT_SET;
- memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat));
- rh.gam_timestamp = timestamp;
-}
-
-/** @internal
-* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
-* @param[in] data Quaternion Adjustment
-* @param[in] timestamp Timestamp of when this is valid
-*/
-void inv_set_compass_correction(const long *data, inv_time_t timestamp)
-{
- rh.status |= INV_COMPASS_CORRECTION_SET;
- memcpy(rh.compass_correction, data, sizeof(rh.compass_correction));
- rh.nav_timestamp = timestamp;
-}
-
-/** @internal
-* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion.
-* @param[out] data Quaternion Adjustment
-* @param[out] timestamp Timestamp of when this is valid
-*/
-void inv_get_compass_correction(long *data, inv_time_t *timestamp)
-{
- memcpy(data, rh.compass_correction, sizeof(rh.compass_correction));
- *timestamp = rh.nav_timestamp;
-}
-
-/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable.
- * @return Returns non-zero if there is a large magnetic field.
- */
-int inv_get_large_mag_field()
-{
- return rh.large_mag_field;
-}
-
-/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable.
- * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large.
- */
-void inv_set_large_mag_field(int state)
-{
- rh.large_mag_field = state;
-}
-
-/** Gets the accel state set by inv_set_acc_state()
- * @return accel state.
- */
-int inv_get_acc_state()
-{
- return rh.acc_state;
-}
-
-/** Sets the accel state. See inv_get_acc_state() to get the value.
- * @param[in] state value to set accel state to.
- */
-void inv_set_acc_state(int state)
-{
- rh.acc_state = state;
- return;
-}
-
-/** Returns the motion state
-* @param[out] cntr Number of previous times a no motion event has occured in a row.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-int inv_get_motion_state(unsigned int *cntr)
-{
- *cntr = rh.motion_state_counter;
- return rh.motion_state;
-}
-
-/** Sets the motion state
- * @param[in] state motion state where INV_NO_MOTION is not moving
- * and INV_MOTION is moving.
- */
-void inv_set_motion_state(unsigned char state)
-{
- long set;
- if (state == rh.motion_state) {
- if (state == INV_NO_MOTION) {
- rh.motion_state_counter++;
- } else {
- rh.motion_state_counter = 0;
- }
- return;
- }
- rh.motion_state_counter = 0;
- rh.motion_state = state;
- /* Equivalent to set = state, but #define's may change. */
- if (state == INV_MOTION)
- set = INV_MSG_MOTION_EVENT;
- else
- set = INV_MSG_NO_MOTION_EVENT;
- inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0);
-}
-
-/** Sets the local earth's magnetic field
-* @param[in] data Local earth's magnetic field in uT scaled by 2^16.
-* Length = 3. Y typically points north, Z typically points down in
-* northern hemisphere and up in southern hemisphere.
-*/
-void inv_set_local_field(const long *data)
-{
- memcpy(rh.local_field, data, sizeof(rh.local_field));
-}
-
-/** Gets the local earth's magnetic field
-* @param[out] data Local earth's magnetic field in uT scaled by 2^16.
-* Length = 3. Y typically points north, Z typically points down in
-* northern hemisphere and up in southern hemisphere.
-*/
-void inv_get_local_field(long *data)
-{
- memcpy(data, rh.local_field, sizeof(rh.local_field));
-}
-
-/** Sets the compass sensitivity
- * @param[in] data Length 3, sensitivity for each compass axis
- * scaled such that 1.0 = 2^30.
- */
-void inv_set_mag_scale(const long *data)
-{
- memcpy(rh.mag_scale, data, sizeof(rh.mag_scale));
-}
-
-/** Gets the compass sensitivity
- * @param[out] data Length 3, sensitivity for each compass axis
- * scaled such that 1.0 = 2^30.
- */
-void inv_get_mag_scale(long *data)
-{
- memcpy(data, rh.mag_scale, sizeof(rh.mag_scale));
-}
-
-/** Gets gravity vector
- * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_get_gravity(long *data)
-{
- data[0] =
- inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]);
- data[1] =
- inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]);
- data[2] =
- (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) -
- 1073741824L;
-
- return INV_SUCCESS;
-}
-
-/** Returns a quaternion based only on gyro and accel.
- * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_get_6axis_quaternion(long *data)
-{
- memcpy(data, rh.gam_quat, sizeof(rh.gam_quat));
- return INV_SUCCESS;
-}
-
-/** Returns a quaternion.
- * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_get_quaternion(long *data)
-{
- if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) {
- inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat);
- rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET);
- }
- memcpy(data, rh.nav_quat, sizeof(rh.nav_quat));
- return INV_SUCCESS;
-}
-
-/** Returns a quaternion.
- * @param[out] data 9-axis quaternion.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_get_quaternion_float(float *data)
-{
- long ldata[4];
- inv_error_t result = inv_get_quaternion(ldata);
- data[0] = inv_q30_to_float(ldata[0]);
- data[1] = inv_q30_to_float(ldata[1]);
- data[2] = inv_q30_to_float(ldata[2]);
- data[3] = inv_q30_to_float(ldata[3]);
- return result;
-}
-
-/** Returns a quaternion with accuracy and timestamp.
- * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30.
- * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate.
- * @param[out] timestamp Timestamp of this quaternion in nanoseconds
- */
-void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp)
-{
- inv_get_quaternion(data);
- *timestamp = inv_get_last_timestamp();
- if (inv_get_compass_on()) {
- *accuracy = inv_get_mag_accuracy();
- } else if (inv_get_gyro_on()) {
- *accuracy = inv_get_gyro_accuracy();
- }else if (inv_get_accel_on()) {
- *accuracy = inv_get_accel_accuracy();
- } else {
- *accuracy = 0;
- }
-}
-
-/** Callback that gets called everytime there is new data. It is
- * registered by inv_start_results_holder().
- * @param[in] sensor_cal New sensor data to process.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal)
-{
- rh.sensor = sensor_cal;
- return INV_SUCCESS;
-}
-
-/** Function to turn on this module. This is automatically called by
- * inv_enable_results_holder(). Typically not called by users.
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_start_results_holder(void)
-{
- inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER,
- INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
- return INV_SUCCESS;
-}
-
-/** Initializes results holder. This is called automatically by the
-* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but
-* is typically not needed to be called by outside callers.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_results_holder(void)
-{
- memset(&rh, 0, sizeof(rh));
- rh.mag_scale[0] = 1L<<30;
- rh.mag_scale[1] = 1L<<30;
- rh.mag_scale[2] = 1L<<30;
- rh.compass_correction[0] = 1L<<30;
- rh.gam_quat[0] = 1L<<30;
- rh.nav_quat[0] = 1L<<30;
- rh.quat_confidence_interval = (float)M_PI;
- return INV_SUCCESS;
-}
-
-/** Turns on storage of results.
-*/
-inv_error_t inv_enable_results_holder()
-{
- inv_error_t result;
- result = inv_init_results_holder();
- if ( result ) {
- return result;
- }
-
- result = inv_register_mpl_start_notification(inv_start_results_holder);
- return result;
-}
-
-/** Sets state of if we know the accel bias.
- * @return return 1 if we know the accel bias, 0 if not.
- * it is set with inv_set_accel_bias_found()
- */
-int inv_got_accel_bias()
-{
- return rh.got_accel_bias;
-}
-
-/** Sets whether we know the accel bias
- * @param[in] state Set to 1 if we know the accel bias.
- * Can be retrieved with inv_got_accel_bias()
- */
-void inv_set_accel_bias_found(int state)
-{
- rh.got_accel_bias = state;
-}
-
-/** Sets state of if we know the compass bias.
- * @return return 1 if we know the compass bias, 0 if not.
- * it is set with inv_set_compass_bias_found()
- */
-int inv_got_compass_bias()
-{
- return rh.got_compass_bias;
-}
-
-/** Sets whether we know the compass bias
- * @param[in] state Set to 1 if we know the compass bias.
- * Can be retrieved with inv_got_compass_bias()
- */
-void inv_set_compass_bias_found(int state)
-{
- rh.got_compass_bias = state;
-}
-
-/** Sets the compass state.
- * @param[in] state Compass state. It can be retrieved with inv_get_compass_state().
- */
-void inv_set_compass_state(int state)
-{
- rh.compass_state = state;
-}
-
-/** Get's the compass state
- * @return the compass state that was set with inv_set_compass_state()
- */
-int inv_get_compass_state()
-{
- return rh.compass_state;
-}
-
-/** Set compass bias error. See inv_get_compass_bias_error()
- * @param[in] bias_error Set's how accurate we know the compass bias. It is the
- * error squared.
- */
-void inv_set_compass_bias_error(const long *bias_error)
-{
- memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error));
-}
-
-/** Get's compass bias error. See inv_set_compass_bias_error() for setting.
- * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared.
- */
-void inv_get_compass_bias_error(long *bias_error)
-{
- memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error));
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer data in body frame
- * with gravity removed
- * @param[out] data 3-element vector of accelerometer data in body frame
- * with gravity removed
- * @return INV_SUCCESS if successful
- * INV_ERROR_INVALID_PARAMETER if invalid input pointer
- */
-inv_error_t inv_get_linear_accel(long *data)
-{
- long gravity[3];
-
- if (data != NULL)
- {
- inv_get_accel_set(data, NULL, NULL);
- inv_get_gravity(gravity);
- data[0] -= gravity[0] >> 14;
- data[1] -= gravity[1] >> 14;
- data[2] -= gravity[2] >> 14;
- return INV_SUCCESS;
- }
- else {
- return INV_ERROR_INVALID_PARAMETER;
- }
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer data in body frame
- * @param[out] data 3-element vector of accelerometer data in body frame
- * @return INV_SUCCESS if successful
- * INV_ERROR_INVALID_PARAMETER if invalid input pointer
- */
-inv_error_t inv_get_accel(long *data)
-{
- if (data != NULL) {
- inv_get_accel_set(data, NULL, NULL);
- return INV_SUCCESS;
- }
- else {
- return INV_ERROR_INVALID_PARAMETER;
- }
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer float data
- * @param[out] data 3-element vector of accelerometer float data
- * @return INV_SUCCESS if successful
- * INV_ERROR_INVALID_PARAMETER if invalid input pointer
- */
-inv_error_t inv_get_accel_float(float *data)
-{
- long tdata[3];
- unsigned char i;
-
- if (data != NULL && !inv_get_accel(tdata)) {
- for (i = 0; i < 3; ++i) {
- data[i] = ((float)tdata[i] / (1L << 16));
- }
- return INV_SUCCESS;
- }
- else {
- return INV_ERROR_INVALID_PARAMETER;
- }
-}
-
-/**
- * @brief Returns 3-element vector of gyro float data
- * @param[out] data 3-element vector of gyro float data
- * @return INV_SUCCESS if successful
- * INV_ERROR_INVALID_PARAMETER if invalid input pointer
- */
-inv_error_t inv_get_gyro_float(float *data)
-{
- long tdata[3];
- unsigned char i;
-
- if (data != NULL) {
- inv_get_gyro_set(tdata, NULL, NULL);
- for (i = 0; i < 3; ++i) {
- data[i] = ((float)tdata[i] / (1L << 16));
- }
- return INV_SUCCESS;
- }
- else {
- return INV_ERROR_INVALID_PARAMETER;
- }
-}
-
-/** Set 9 axis 95% heading confidence interval for quaternion
-* @param[in] ci Confidence interval in radians.
-*/
-void inv_set_heading_confidence_interval(float ci)
-{
- rh.quat_confidence_interval = ci;
-}
-
-/** Get 9 axis 95% heading confidence interval for quaternion
-* @return Confidence interval in radians.
-*/
-float inv_get_heading_confidence_interval(void)
-{
- return rh.quat_confidence_interval;
-}
-
-/**
- * @brief Returns 3-element vector of linear accel float data
- * @param[out] data 3-element vector of linear aceel float data
- * @return INV_SUCCESS if successful
- * INV_ERROR_INVALID_PARAMETER if invalid input pointer
- */
-inv_error_t inv_get_linear_accel_float(float *data)
-{
- long tdata[3];
- unsigned char i;
-
- if (data != NULL && !inv_get_linear_accel(tdata)) {
- for (i = 0; i < 3; ++i) {
- data[i] = ((float)tdata[i] / (1L << 16));
- }
- return INV_SUCCESS;
- }
- else {
- return INV_ERROR_INVALID_PARAMETER;
- }
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.h b/60xx/libsensors_iio/software/core/mllite/results_holder.h
deleted file mode 100644
index 09da24e..0000000
--- a/60xx/libsensors_iio/software/core/mllite/results_holder.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#include "mltypes.h"
-
-#ifndef INV_RESULTS_HOLDER_H__
-#define INV_RESULTS_HOLDER_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define INV_MOTION 0x0001
-#define INV_NO_MOTION 0x0002
-
- /**************************************************************************/
- /* The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = */
- /* 2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even. */
- /* The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = */
- /* 2^ACC_MAG_SQR_SHIFT */
- /**************************************************************************/
-#define ACC_MAG_SQR_SHIFT 16
-
-void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp);
-
-// States
-#define SF_NORMAL 0
-#define SF_UNCALIBRATED 1
-#define SF_STARTUP_SETTLE 2
-#define SF_FAST_SETTLE 3
-#define SF_DISTURBANCE 4
-#define SF_SLOW_SETTLE 5
-
-int inv_get_acc_state();
-void inv_set_acc_state(int state);
-int inv_get_motion_state(unsigned int *cntr);
-void inv_set_motion_state(unsigned char state);
-inv_error_t inv_get_gravity(long *data);
-inv_error_t inv_get_6axis_quaternion(long *data);
-inv_error_t inv_get_quaternion(long *data);
-inv_error_t inv_get_quaternion_float(float *data);
-void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp);
-
-inv_error_t inv_enable_results_holder();
-inv_error_t inv_init_results_holder(void);
-
-/* Magnetic Field Parameters*/
-void inv_set_local_field(const long *data);
-void inv_get_local_field(long *data);
-void inv_set_mag_scale(const long *data);
-void inv_get_mag_scale(long *data);
-void inv_set_compass_correction(const long *data, inv_time_t timestamp);
-void inv_get_compass_correction(long *data, inv_time_t *timestamp);
-int inv_got_compass_bias();
-void inv_set_compass_bias_found(int state);
-int inv_get_large_mag_field();
-void inv_set_large_mag_field(int state);
-void inv_set_compass_state(int state);
-int inv_get_compass_state();
-void inv_set_compass_bias_error(const long *bias_error);
-void inv_get_compass_bias_error(long *bias_error);
-inv_error_t inv_get_linear_accel(long *data);
-inv_error_t inv_get_accel(long *data);
-inv_error_t inv_get_accel_float(float *data);
-inv_error_t inv_get_gyro_float(float *data);
-inv_error_t inv_get_linear_accel_float(float *data);
-void inv_set_heading_confidence_interval(float ci);
-float inv_get_heading_confidence_interval(void);
-
-int inv_got_accel_bias();
-void inv_set_accel_bias_found(int state);
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_RESULTS_HOLDER_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.c b/60xx/libsensors_iio/software/core/mllite/start_manager.c
deleted file mode 100644
index fb758e7..0000000
--- a/60xx/libsensors_iio/software/core/mllite/start_manager.c
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-/**
- * @defgroup Start_Manager start_manager
- * @brief Motion Library - Start Manager
- * Start Manager
- *
- * @{
- * @file start_manager.c
- * @brief This handles all the callbacks when inv_start_mpl() is called.
- */
-
-
-#include <string.h>
-#include "log.h"
-#include "start_manager.h"
-
-typedef inv_error_t (*inv_start_cb_func)();
-struct inv_start_cb_t {
- int num_cb;
- inv_start_cb_func start_cb[INV_MAX_START_CB];
-};
-
-static struct inv_start_cb_t inv_start_cb;
-
-/** Initilize the start manager. Typically called by inv_start_mpl();
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_start_manager(void)
-{
- memset(&inv_start_cb, 0, sizeof(inv_start_cb));
- return INV_SUCCESS;
-}
-
-/** Removes a callback from start notification
-* @param[in] start_cb function to remove from start notification
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- int kk;
-
- for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
- if (inv_start_cb.start_cb[kk] == start_cb) {
- // Found the match
- if (kk != (inv_start_cb.num_cb-1)) {
- memmove(&inv_start_cb.start_cb[kk],
- &inv_start_cb.start_cb[kk+1],
- (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
- }
- inv_start_cb.num_cb--;
- return INV_SUCCESS;
- }
- }
- return INV_ERROR_INVALID_PARAMETER;
-}
-
-/** Register a callback to receive when inv_start_mpl() is called.
-* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- if (inv_start_cb.num_cb >= INV_MAX_START_CB)
- return INV_ERROR_INVALID_PARAMETER;
-
- inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
- inv_start_cb.num_cb++;
- return INV_SUCCESS;
-}
-
-/** Callback all the functions that want to be notified when inv_start_mpl() was
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_execute_mpl_start_notification(void)
-{
- inv_error_t result,first_error;
- int kk;
-
- first_error = INV_SUCCESS;
-
- for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
- result = inv_start_cb.start_cb[kk]();
- if (result && (first_error == INV_SUCCESS)) {
- first_error = result;
- }
- }
- return first_error;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.h b/60xx/libsensors_iio/software/core/mllite/start_manager.h
deleted file mode 100644
index 899e3f5..0000000
--- a/60xx/libsensors_iio/software/core/mllite/start_manager.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_START_MANAGER_H__
-#define INV_START_MANAGER_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-
-/** Max number of start callbacks we can handle. */
-#define INV_MAX_START_CB 20
-
-inv_error_t inv_init_start_manager(void);
-inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void));
-inv_error_t inv_execute_mpl_start_notification(void);
-inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void));
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INV_START_MANAGER_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c
deleted file mode 100644
index 3e4e619..0000000
--- a/60xx/libsensors_iio/software/core/mllite/storage_manager.c
+++ /dev/null
@@ -1,204 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/**
- * @defgroup Storage_Manager storage_manager
- * @brief Motion Library - Stores Data for functions.
- *
- *
- * @{
- * @file storage_manager.c
- * @brief Load and Store Manager.
- */
-
-#include <string.h>
-
-#include "storage_manager.h"
-#include "log.h"
-#include "ml_math_func.h"
-#include "mlmath.h"
-
-/* Must be changed if the format of storage changes */
-#define DEFAULT_KEY 29681
-
-typedef inv_error_t (*load_func_t)(const unsigned char *data);
-typedef inv_error_t (*save_func_t)(unsigned char *data);
-/** Max number of entites that can be stored */
-#define NUM_STORAGE_BOXES 20
-
-struct data_header_t {
- long size;
- uint32_t checksum;
- unsigned int key;
-};
-
-struct data_storage_t {
- int num; /**< Number of differnt save entities */
- size_t total_size; /**< Size in bytes to store non volatile data */
- load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */
- save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */
- struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */
-};
-static struct data_storage_t ds;
-
-/** Should be called once before using any of the storage methods. Typically
-* called first by inv_init_mpl().*/
-void inv_init_storage_manager()
-{
- memset(&ds, 0, sizeof(ds));
- ds.total_size = sizeof(struct data_header_t);
-}
-
-/** Used to register your mechanism to load and store non-volative data. This should typical be
-* called during the enable function for your feature.
-* @param[in] load_func function pointer you will use to receive data that was stored for you.
-* @param[in] save_func function pointer you will use to save any data you want saved to
-* non-volatile memory between runs.
-* @param[in] size The size in bytes of the amount of data you want loaded and saved.
-* @param[in] key The key associated with your data type should be unique across MPL.
-* The key should change when your type of data for storage changes.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data),
- inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key)
-{
- int kk;
- // Check if this has been registered already
- for (kk=0; kk<ds.num; ++kk) {
- if (key == ds.hd[kk].key) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- }
- // Make sure there is room
- if (ds.num >= NUM_STORAGE_BOXES) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- // Add to list
- ds.hd[ds.num].key = key;
- ds.hd[ds.num].size = size;
- ds.load[ds.num] = load_func;
- ds.save[ds.num] = save_func;
- ds.total_size += size + sizeof(struct data_header_t);
- ds.num++;
-
- return INV_SUCCESS;
-}
-
-/** Returns the memory size needed to perform a store
-* @param[out] size Size in bytes of memory needed to store.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_get_mpl_state_size(size_t *size)
-{
- *size = ds.total_size;
- return INV_SUCCESS;
-}
-
-/** @internal
- * Finds key in ds.hd[] array and returns location
- * @return location where key exists in array, -1 if not found.
- */
-static int inv_find_entry(unsigned int key)
-{
- int kk;
- for (kk=0; kk<ds.num; ++kk) {
- if (key == ds.hd[kk].key) {
- return kk;
- }
- }
- return -1;
-}
-
-/** This function takes a block of data that has been saved in non-volatile memory and pushes
-* to the proper locations. Multiple error checks are performed on the data.
-* @param[in] data Data that was saved to be loaded up by MPL
-* @param[in] length Length of data vector in bytes
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length)
-{
- struct data_header_t *hd;
- int entry;
- uint32_t checksum;
- long len;
-
- len = length; // Important so we get negative numbers
- if (len < sizeof(struct data_header_t))
- return INV_ERROR_CALIBRATION_LOAD; // No data
- hd = (struct data_header_t *)data;
- if (hd->key != DEFAULT_KEY)
- return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption
- len = MIN(hd->size, len);
- len = hd->size;
- len -= sizeof(struct data_header_t);
- data += sizeof(struct data_header_t);
- checksum = inv_checksum(data, len);
- if (checksum != hd->checksum)
- return INV_ERROR_CALIBRATION_LOAD; // Data corruption
-
- while (len > (long)sizeof(struct data_header_t)) {
- hd = (struct data_header_t *)data;
- entry = inv_find_entry(hd->key);
- data += sizeof(struct data_header_t);
- len -= sizeof(struct data_header_t);
- if (entry >= 0 && len >= hd->size) {
- if (hd->size == ds.hd[entry].size) {
- checksum = inv_checksum(data, hd->size);
- if (checksum == hd->checksum) {
- ds.load[entry](data);
- } else {
- return INV_ERROR_CALIBRATION_LOAD;
- }
- }
- }
- len -= hd->size;
- if (len >= 0)
- data = data + hd->size;
- }
-
- return INV_SUCCESS;
-}
-
-/** This function fills up a block of memory to be stored in non-volatile memory.
-* @param[out] data Place to store data, size of sz, must be at least size
-* returned by inv_get_mpl_state_size()
-* @param[in] sz Size of data.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz)
-{
- unsigned char *cur;
- int kk;
- struct data_header_t *hd;
-
- if (sz >= ds.total_size) {
- cur = data + sizeof(struct data_header_t);
- for (kk = 0; kk < ds.num; ++kk) {
- hd = (struct data_header_t *)cur;
- cur += sizeof(struct data_header_t);
- ds.save[kk](cur);
- hd->checksum = inv_checksum(cur, ds.hd[kk].size);
- hd->size = ds.hd[kk].size;
- hd->key = ds.hd[kk].key;
- cur += ds.hd[kk].size;
- }
- } else {
- return INV_ERROR_CALIBRATION_LOAD;
- }
-
- hd = (struct data_header_t *)data;
- hd->checksum = inv_checksum(data + sizeof(struct data_header_t),
- ds.total_size - sizeof(struct data_header_t));
- hd->key = DEFAULT_KEY;
- hd->size = ds.total_size;
-
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.h b/60xx/libsensors_iio/software/core/mllite/storage_manager.h
deleted file mode 100644
index 7255591..0000000
--- a/60xx/libsensors_iio/software/core/mllite/storage_manager.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#include "mltypes.h"
-
-#ifndef INV_STORAGE_MANAGER_H__
-#define INV_STORAGE_MANAGER_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_register_load_store(
- inv_error_t (*load_func)(const unsigned char *data),
- inv_error_t (*save_func)(unsigned char *data),
- size_t size, unsigned int key);
-void inv_init_storage_manager(void);
-
-inv_error_t inv_get_mpl_state_size(size_t *size);
-inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len);
-inv_error_t inv_save_mpl_states(unsigned char *data, size_t len);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* INV_STORAGE_MANAGER_H__ */
diff --git a/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h
deleted file mode 100644
index 5a53213..0000000
--- a/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id$
- *
- ******************************************************************************/
-
-#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__
-#define MLDMP_ACCEL_AUTO_CALIBRATION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_in_use_auto_calibration(void);
-inv_error_t inv_disable_in_use_auto_calibration(void);
-inv_error_t inv_stop_in_use_auto_calibration(void);
-inv_error_t inv_start_in_use_auto_calibration(void);
-inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
-inv_error_t inv_init_in_use_auto_calibration(void);
-void inv_init_accel_maxmin(void);
-void inv_record_good_accel_maxmin(void);
-int inv_get_accel_bias_stage();
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__
-
diff --git a/60xx/libsensors_iio/software/core/mpl/authenticate.h b/60xx/libsensors_iio/software/core/mpl/authenticate.h
deleted file mode 100644
index d7c803b..0000000
--- a/60xx/libsensors_iio/software/core/mpl/authenticate.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id$
- *
- ******************************************************************************/
-
-#ifndef MLDMP_AUTHENTICATE_H__
-#define MLDMP_AUTHENTICATE_H__
-
-#include "invensense.h"
-
-inv_error_t inv_check_key(void);
-
-#endif /* MLDMP_AUTHENTICATE_H__ */
diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
deleted file mode 100644
index fbd346f..0000000
--- a/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so
+++ /dev/null
Binary files differ
diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk
deleted file mode 100644
index 2e15205..0000000
--- a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk
+++ /dev/null
@@ -1,88 +0,0 @@
-MPL_LIB_NAME ?= mplmpu
-LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT)
-
-MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST)))
-
-CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi-
-COMP ?= $(CROSS)gcc
-LINK ?= $(CROSS)gcc
-
-OBJFOLDER = $(CURDIR)/obj
-
-INV_ROOT = ../../../../..
-MLLITE_DIR = $(INV_ROOT)/software/core/mllite
-MPL_DIR = $(INV_ROOT)/software/core/mpl
-
-include $(INV_ROOT)/software/build/android/common.mk
-
-CFLAGS += $(CMDLINE_CFLAGS)
-CFLAGS += $(ANDROID_COMPILE)
-CFLAGS += -Wall
-CFLAGS += -fpic
-CFLAGS += -nostdlib
-CFLAGS += -DNDEBUG
-CFLAGS += -D_REENTRANT
-CFLAGS += -DLINUX
-CFLAGS += -DANDROID
-CFLAGS += -mthumb-interwork
-CFLAGS += -fno-exceptions
-CFLAGS += -ffunction-sections
-CFLAGS += -funwind-tables
-CFLAGS += -fstack-protector
-CFLAGS += -fno-short-enums
-CFLAGS += -fmessage-length=0
-CFLAGS += $(INV_INCLUDES)
-CFLAGS += $(INV_DEFINES)
-
-LLINK = -lc
-LLINK += -lm
-LLINK += -lutils
-LLINK += -lcutils
-LLINK += -lgcc
-LLINK += -ldl
-
-LFLAGS += $(CMDLINE_LFLAGS)
-LFLAGS += -shared
-LFLAGS += -Wl,-soname,$(LIBRARY)
-LFLAGS += -Wl,-shared,-Bsymbolic
-LFLAGS += $(ANDROID_LINK)
-LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib
-
-####################################################################################################
-## sources
-
-INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT)
-
-#INV_SOURCES, VPATH provided by Makefile.filelist
-include ../filelist.mk
-
-INV_OBJS := $(addsuffix .o,$(INV_SOURCES))
-INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES))))
-
-####################################################################################################
-## rules
-
-.PHONY: all mpl clean cleanall
-
-all: mpl
-
-mpl: $(LIBRARY) $(MK_NAME)
-
-$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME)
- @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n")
- $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK)
-
-$(OBJFOLDER) :
- @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n")
- mkdir obj
-
-$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME)
- @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n")
- $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $<
-
-clean :
- rm -fR $(OBJFOLDER)
-
-cleanall :
- rm -fR $(LIBRARY) $(OBJFOLDER)
-
diff --git a/60xx/libsensors_iio/software/core/mpl/build/filelist.mk b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk
deleted file mode 100644
index e18003a..0000000
--- a/60xx/libsensors_iio/software/core/mpl/build/filelist.mk
+++ /dev/null
@@ -1,46 +0,0 @@
-#### filelist.mk for mpl ####
-
-# headers only
-HEADERS := $(MPL_DIR)/mpu.h
-
-# headers for sources
-HEADERS := $(MPL_DIR)/fast_no_motion.h
-HEADERS += $(MPL_DIR)/fusion_9axis.h
-HEADERS += $(MPL_DIR)/motion_no_motion.h
-HEADERS += $(MPL_DIR)/no_gyro_fusion.h
-HEADERS += $(MPL_DIR)/quaternion_supervisor.h
-HEADERS += $(MPL_DIR)/gyro_tc.h
-HEADERS += $(MPL_DIR)/authenticate.h
-HEADERS += $(MPL_DIR)/accel_auto_cal.h
-HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h
-HEADERS += $(MPL_DIR)/compass_vec_cal.h
-HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h
-HEADERS += $(MPL_DIR)/mag_disturb.h
-HEADERS += $(MPL_DIR)/mag_disturb_protected.h
-HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h
-HEADERS += $(MPL_DIR)/heading_from_gyro.h
-HEADERS += $(MPL_DIR)/compass_fit.h
-HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h
-#HEADERS += $(MPL_DIR)/auto_calibration.h
-
-# sources
-SOURCES := $(MPL_DIR)/fast_no_motion.c
-SOURCES += $(MPL_DIR)/fusion_9axis.c
-SOURCES += $(MPL_DIR)/motion_no_motion.c
-SOURCES += $(MPL_DIR)/no_gyro_fusion.c
-SOURCES += $(MPL_DIR)/quaternion_supervisor.c
-SOURCES += $(MPL_DIR)/gyro_tc.c
-SOURCES += $(MPL_DIR)/authenticate.c
-SOURCES += $(MPL_DIR)/accel_auto_cal.c
-SOURCES += $(MPL_DIR)/compass_vec_cal.c
-SOURCES += $(MPL_DIR)/mag_disturb.c
-SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c
-SOURCES += $(MPL_DIR)/heading_from_gyro.c
-SOURCES += $(MPL_DIR)/compass_fit.c
-SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c
-#SOURCES += $(MPL_DIR)/auto_calibration.c
-
-INV_SOURCES += $(SOURCES)
-
-VPATH = $(MPL_DIR)
-
diff --git a/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
deleted file mode 100644
index 4f01fc2..0000000
--- a/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_COMPASSBIASWGYRO_H__
-#define MLDMP_COMPASSBIASWGYRO_H__
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_enable_compass_bias_w_gyro();
- inv_error_t inv_disable_compass_bias_w_gyro();
- void inv_init_compass_bias_w_gyro();
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_COMPASSBIASWGYRO_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/compass_fit.h b/60xx/libsensors_iio/software/core/mpl/compass_fit.h
deleted file mode 100644
index be3cce8..0000000
--- a/60xx/libsensors_iio/software/core/mpl/compass_fit.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-#ifndef INV_MLDMP_COMPASSFIT_H__
-#define INV_MLDMP_COMPASSFIT_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void inv_init_compass_fit(void);
-inv_error_t inv_enable_compass_fit(void);
-inv_error_t inv_disable_compass_fit(void);
-inv_error_t inv_start_compass_fit(void);
-inv_error_t inv_stop_compass_fit(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // INV_MLDMP_COMPASSFIT_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
deleted file mode 100644
index a3e044c..0000000
--- a/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-#ifndef COMPASS_ONLY_CAL_H__
-#define COMPASS_ONLY_CAL_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_vector_compass_cal();
-inv_error_t inv_disable_vector_compass_cal();
-inv_error_t inv_start_vector_compass_cal(void);
-inv_error_t inv_stop_vector_compass_cal(void);
-void inv_vector_compass_cal_sensitivity(float sens);
-inv_error_t inv_init_vector_compass_cal();
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // COMPASS_ONLY_CAL_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h
deleted file mode 100644
index c553926..0000000
--- a/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_FAST_NO_MOTION_H__
-#define MLDMP_FAST_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_enable_fast_nomot(void);
- inv_error_t inv_disable_fast_nomot(void);
- inv_error_t inv_start_fast_nomot(void);
- inv_error_t inv_stop_fast_nomot(void);
- inv_error_t inv_init_fast_nomot(void);
- void inv_set_default_number_of_samples(int N);
- inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);
- inv_error_t inv_update_fast_nomot(long *gyro);
-
- void inv_get_fast_nomot_accel_param(long *cntr, long long *param);
- void inv_get_fast_nomot_compass_param(long *cntr, long long *param);
- void inv_set_fast_nomot_accel_threshold(long long thresh);
- void inv_set_fast_nomot_compass_threshold(long long thresh);
- void int_set_fast_nomot_gyro_threshold(long long thresh);
-
- void inv_fnm_debug_print(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_FAST_NO_MOTION_H__
-
diff --git a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
deleted file mode 100644
index 1ba1ebb..0000000
--- a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_FUSION9AXIS_H__
-#define MLDMP_FUSION9AXIS_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- void inv_init_9x_fusion(void);
- inv_error_t inv_9x_fusion_state_change(unsigned char newState);
- inv_error_t inv_enable_9x_sensor_fusion(void);
- inv_error_t inv_disable_9x_sensor_fusion(void);
- inv_error_t inv_start_9x_sensor_fusion(void);
- inv_error_t inv_stop_9x_sensor_fusion(void);
- inv_error_t inv_9x_fusion_set_mag_fb(double fb);
- inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_FUSION9AXIS_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
deleted file mode 100644
index 69918d5..0000000
--- a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef _GYRO_TC_H_
-#define _GYRO_TC_H_
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_gyro_tc(void);
-inv_error_t inv_disable_gyro_tc(void);
-inv_error_t inv_start_gyro_tc(void);
-inv_error_t inv_stop_gyro_tc(void);
-
-inv_error_t inv_get_gyro_ts(long *data);
-inv_error_t inv_set_gyro_ts(long *data);
-
-inv_error_t inv_init_gyro_ts(void);
-
-inv_error_t inv_set_gtc_max_temp(long data);
-inv_error_t inv_set_gtc_min_temp(long data);
-
-inv_error_t inv_print_gtc_data(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _GYRO_TC_H */
-
diff --git a/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
deleted file mode 100644
index 09ecc42..0000000
--- a/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef _HEADING_FROM_GYRO_H_
-#define _HEADING_FROM_GYRO_H_
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_enable_heading_from_gyro(void);
- inv_error_t inv_disable_heading_from_gyro(void);
- void inv_init_heading_from_gyro(void);
- inv_error_t inv_start_heading_from_gyro(void);
- inv_error_t inv_stop_heading_from_gyro(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif /* _HEADING_FROM_GYRO_H_ */
diff --git a/60xx/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h
deleted file mode 100644
index 6620bbf..0000000
--- a/60xx/libsensors_iio/software/core/mpl/inv_math.h
+++ /dev/null
@@ -1,8 +0,0 @@
-/* math.h has many functions and defines that are not consistent across
-* platforms. This address that */
-
-#ifdef _WINDOWS
-#define _USE_MATH_DEFINES
-#endif
-
-#include <math.h>
diff --git a/60xx/libsensors_iio/software/core/mpl/invensense_adv.h b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h
deleted file mode 100644
index 12932c9..0000000
--- a/60xx/libsensors_iio/software/core/mpl/invensense_adv.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- $License:
- Copyright (C) 2012 InvenSense Corporation, All Rights Reserved.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-/*
- Main header file for Invensense's Advanced library.
-*/
-
-#include "accel_auto_cal.h"
-#include "compass_bias_w_gyro.h"
-#include "compass_fit.h"
-#include "compass_vec_cal.h"
-#include "fast_no_motion.h"
-#include "fusion_9axis.h"
-#include "gyro_tc.h"
-#include "heading_from_gyro.h"
-#include "mag_disturb.h"
-#include "motion_no_motion.h"
-#include "no_gyro_fusion.h"
-#include "quaternion_supervisor.h"
-#include "mag_disturb.h"
-#include "quat_accuracy_monitor.h"
-#include "shake.h"
diff --git a/60xx/libsensors_iio/software/core/mpl/mag_disturb.h b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h
deleted file mode 100644
index 38df919..0000000
--- a/60xx/libsensors_iio/software/core/mpl/mag_disturb.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-#ifndef MLDMP_MAGDISTURB_H__
-#define MLDMP_MAGDISTURB_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat,
- const long *compass, const long *gravity);
-
- void inv_track_dip_angle(int mode, float currdip);
-
- inv_error_t inv_enable_magnetic_disturbance(void);
- inv_error_t inv_disable_magnetic_disturbance(void);
- int inv_get_magnetic_disturbance_state();
- inv_error_t inv_set_magnetic_disturbance(int time_ms);
- inv_error_t inv_disable_dip_tracking(void);
- inv_error_t inv_enable_dip_tracking(void);
- inv_error_t inv_init_magnetic_disturbance(void);
-
- float Mag3ofNormalizedLong(const long *x);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_MAGDISTURB_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
deleted file mode 100644
index 01cf1c0..0000000
--- a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_MOTION_NO_MOTION_H__
-#define INV_MOTION_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_motion_no_motion(void);
-inv_error_t inv_disable_motion_no_motion(void);
-inv_error_t inv_init_motion_no_motion(void);
-inv_error_t inv_start_motion_no_motion(void);
-inv_error_t inv_stop_motion_no_motion(void);
-
-inv_error_t inv_set_no_motion_time(long time_ms);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MOTION_NO_MOTION_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
deleted file mode 100644
index 38d5690..0000000
--- a/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-
-/******************************************************************************
- *
- * $Id$
- *
- *****************************************************************************/
-
-#ifndef MLDMP_NOGYROFUSION_H__
-#define MLDMP_NOGYROFUSION_H__
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_enable_no_gyro_fusion(void);
- inv_error_t inv_disable_no_gyro_fusion(void);
- inv_error_t inv_start_no_gyro_fusion(void);
- inv_error_t inv_start_no_gyro_fusion(void);
- inv_error_t inv_init_no_gyro_fusion(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_NOGYROFUSION_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
deleted file mode 100644
index 3c6a2c1..0000000
--- a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- quat_accuracy_monitor.h
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-#ifndef QUAT_ACCURARCY_MONITOR_H__
-#define QUAT_ACCURARCY_MONITOR_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-enum accuracy_signal_type_e {
- TYPE_NAV_QUAT,
- TYPE_GAM_QUAT,
- TYPE_NAV_QUAT_ADVANCED,
- TYPE_GAM_QUAT_ADVANCED,
- TYPE_NAV_QUAT_BASIC,
- TYPE_GAM_QUAT_BASIC,
- TYPE_MAG,
- TYPE_GYRO,
- TYPE_ACCEL,
-};
-
-inv_error_t inv_init_quat_accuracy_monitor(void);
-
-void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
-double get_accuracy_threshold(enum accuracy_signal_type_e type);
-void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
-int get_accuracy_weight(enum accuracy_signal_type_e type);
-
-int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
-
-void inv_reset_quat_accuracy(void);
-double get_6axis_correction_term(void);
-double get_9axis_correction_term(void);
-int get_9axis_accuracy_state();
-
-void set_6axis_error_average(double value);
-double get_6axis_error_bound(void);
-double get_compass_correction(void);
-double get_9axis_error_bound(void);
-
-float get_confidence_interval(void);
-void set_compass_uncertainty(float value);
-
-inv_error_t inv_enable_quat_accuracy_monitor(void);
-inv_error_t inv_disable_quat_accuracy_monitor(void);
-inv_error_t inv_start_quat_accuracy_monitor(void);
-inv_error_t inv_stop_quat_accuracy_monitor(void);
-
-double get_compassNgravity(void);
-double get_init_compassNgravity(void);
-
-float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
deleted file mode 100644
index 4fa36b0..0000000
--- a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_QUATERNION_SUPERVISOR_H__
-#define INV_QUATERNION_SUPERVISOR_H__
-
-#include "mltypes.h"
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_quaternion(void);
-inv_error_t inv_disable_quaternion(void);
-inv_error_t inv_init_quaternion(void);
-inv_error_t inv_start_quaternion(void);
-void inv_set_quaternion(long *quat);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h
deleted file mode 100644
index 67acb7b..0000000
--- a/60xx/libsensors_iio/software/core/mpl/shake.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_SHAKE_H__
-#define INV_SHAKE_H__
-
-#include "mltypes.h"
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- #define STATE_ZERO 0
- #define STATE_INIT_1 1
- #define STATE_INIT_2 2
- #define STATE_DETECT 3
-
- struct t_shake_config_params {
- long shake_time_min_ms;
- long shake_time_max_ms;
- long shake_time_min;
- long shake_time_max;
- unsigned char shake_time_set;
- long shake_time_saved;
- float shake_deriv_thr;
- int zero_cross_thr;
- float accel_delta_min;
- float accel_delta_max;
- unsigned char interp_enable;
- };
-
- struct t_shake_state_params {
- unsigned char state;
- float accel_peak_high;
- float accel_peak_low;
- float accel_range;
- int num_zero_cross;
- short curr_shake_time;
- int deriv_major_change;
- int deriv_major_sign;
- float accel_buffer[200];
- float delta_buffer[200];
- };
-
- struct t_shake_data_params {
- float accel_prev;
- float accel_curr;
- float delta_prev;
- float delta_curr;
- float delta_prev_buffer;
- };
-
- struct t_shake_results {
- //unsigned char shake_int;
- int shake_number;
- };
-
- struct t_shake_cb {
- void (*shake_callback)(struct t_shake_results *shake_results);
- };
-
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
- inv_error_t inv_enable_shake(void);
- inv_error_t inv_disable_shake(void);
- inv_error_t inv_init_shake(void);
- inv_error_t inv_start_shake(void);
- int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
- void inv_config_shake_time_params(long sample_time_ms);
- void inv_set_shake_accel_delta_min(float accel_g);
- void inv_set_shake_accel_delta_max(float accel_g);
- void inv_set_shake_zero_cross_thresh(int num_zero_cross);
- void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
- void inv_set_shake_time_min_ms(long time_ms);
- void inv_set_shake_time_max_ms(long time_ms);
- void inv_enable_shake_data_interpolation(unsigned char en);
-
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_SHAKE__
\ No newline at end of file
diff --git a/60xx/mlsdk/Android.mk b/60xx/mlsdk/Android.mk
deleted file mode 100644
index 929a776..0000000
--- a/60xx/mlsdk/Android.mk
+++ /dev/null
@@ -1,101 +0,0 @@
-LOCAL_PATH := $(call my-dir)
-
-ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false)
-
-include $(CLEAR_VARS)
-LOCAL_MODULE_TAGS := optional
-
-LOCAL_MODULE := libmlplatform
-#modify these to point to the mpl source installation
-MLSDK_ROOT = .
-MLPLATFORM_DIR = $(MLSDK_ROOT)/platform/linux
-
-LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID
-LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/kernel
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite
-
-ML_SOURCES := \
- $(MLPLATFORM_DIR)/mlos_linux.c \
- $(MLPLATFORM_DIR)/mlsl_linux_mpu.c
-
-LOCAL_SRC_FILES := $(ML_SOURCES)
-
-LOCAL_SHARED_LIBRARIES := liblog libm libutils libcutils
-include $(BUILD_SHARED_LIBRARY)
-
-include $(CLEAR_VARS)
-LOCAL_MODULE := libmllite
-LOCAL_MODULE_TAGS := optional
-#modify these to point to the mpl source installation
-MLSDK_ROOT = .
-MLPLATFORM_DIR = $(MLSDK_ROOT)/platform
-MLLITE_DIR = $(MLSDK_ROOT)/mllite
-MPL_DIR = $(MLSDK_ROOT)/mldmp
-
-LOCAL_CFLAGS += -DNDEBUG
-LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID
-LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050
-LOCAL_CFLAGS += -DUNICODE -D_UNICODE -DSK_RELEASE
-LOCAL_CFLAGS += -DI2CDEV=\"/dev/mpu\"
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MPL_DIR) -I$(LOCAL_PATH)/$(MLLITE_DIR) -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/include
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlutils -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlapps/common
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite/akmd
-LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/linux
-
-# optionally apply the compass filter. this is set in
-# BoardConfig.mk
-ifeq ($(BOARD_INVENSENSE_APPLY_COMPASS_NOISE_FILTER),true)
-LOCAL_CFLAGS += -DAPPLY_COMPASS_FILTER
-endif
-
-ML_SOURCES = \
- $(MLLITE_DIR)/accel.c \
- $(MLLITE_DIR)/compass.c \
- $(MLLITE_DIR)/pressure.c \
- $(MLLITE_DIR)/mldl_cfg_mpu.c \
- $(MLLITE_DIR)/dmpDefault.c \
- $(MLLITE_DIR)/ml.c \
- $(MLLITE_DIR)/mlarray.c \
- $(MLLITE_DIR)/mlarray_legacy.c \
- $(MLLITE_DIR)/mlFIFO.c \
- $(MLLITE_DIR)/mlFIFOHW.c \
- $(MLLITE_DIR)/mlMathFunc.c \
- $(MLLITE_DIR)/ml_stored_data.c \
- $(MLLITE_DIR)/mlcontrol.c \
- $(MLLITE_DIR)/mldl.c \
- $(MLLITE_DIR)/mldmp.c \
- $(MLLITE_DIR)/mlstates.c \
- $(MLLITE_DIR)/mlsupervisor.c \
- $(MLLITE_DIR)/mlBiasNoMotion.c \
- $(MLLITE_DIR)/mlSetGyroBias.c \
- \
- $(MLLITE_DIR)/ml_mputest.c \
- $(MLSDK_ROOT)/mlutils/mputest.c \
- $(MLSDK_ROOT)/mlutils/checksum.c
-
-
-ifeq ($(HARDWARE),M_HW)
- ML_SOURCES += $(MLLITE_DIR)/accel/mantis.c
-endif
-
-LOCAL_SRC_FILES := $(ML_SOURCES)
-LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform
-include $(BUILD_SHARED_LIBRARY)
-
-#This makes an .so from our .a
-#include $(CLEAR_VARS)
-#LOCAL_MODULE := libmpl
-#LOCAL_MODULE_TAGS := optional
-#LOCAL_SRC_FILES := mlsdk/mldmp/mpl/android/libmpl.a
-#LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform libmllite
-#LOCAL_WHOLE_STATIC_LIBRARIES := libmpl
-#LOCAL_PREBUILT_LIBS := mlsdk/mldmp/mpl/android/libmpl.a
-#include $(BUILD_SHARED_LIBRARY)
-#include $(BUILD_MULTI_PREBUILT)
-
-endif
diff --git a/60xx/mlsdk/mllite/accel.c b/60xx/mlsdk/mllite/accel.c
deleted file mode 100644
index 60b8d6c..0000000
--- a/60xx/mlsdk/mllite/accel.c
+++ /dev/null
@@ -1,189 +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: accel.c 4595 2011-01-25 01:43:03Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- * @defgroup ACCELDL
- * @brief Motion Library - Accel Driver Layer.
- * Provides the interface to setup and handle an accel
- * connected to either the primary or the seconday I2C interface
- * of the gyroscope.
- *
- * @{
- * @file accel.c
- * @brief Accel setup and handling methods.
-**/
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.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-accel"
-
-#define ACCEL_DEBUG 0
-
-/* --------------------- */
-/* - Global Variables. - */
-/* --------------------- */
-
-/* --------------------- */
-/* - Static Variables. - */
-/* --------------------- */
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* -------------- */
-/* - Externs. - */
-/* -------------- */
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/**
- * @brief Used to determine if an accel is configured and
- * used by the MPL.
- * @return INV_SUCCESS if the accel is present.
- */
-unsigned char inv_accel_present(void)
-{
- INVENSENSE_FUNC_START;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- if (NULL != mldl_cfg->accel &&
- NULL != mldl_cfg->accel->resume &&
- mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL)
- return TRUE;
- else
- return FALSE;
-}
-
-/**
- * @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
- * X, Y, and Z axes.
- * @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_get_accel_data(long *data)
-{
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- inv_error_t result;
- unsigned char raw_data[2 * ACCEL_NUM_AXES];
- long tmp[ACCEL_NUM_AXES];
- int ii;
- signed char *mtx = mldl_cfg->pdata->accel.orientation;
- char accelId = mldl_cfg->accel->id;
-
- if (NULL == data)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (mldl_cfg->accel->read_len > sizeof(raw_data))
- return INV_ERROR_ASSERTION_FAILURE;
-
- result = (inv_error_t) inv_mpu_read_accel(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(),
- raw_data);
- if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
- return result;
- }
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
- if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) {
- tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256;
- tmp[ii] += (long)((unsigned char)raw_data[2 * ii]);
- } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) ||
- (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) {
- tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256;
- tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]);
- if (accelId == ACCEL_ID_KXSD9) {
- tmp[ii] = (long)((short)(((unsigned short)tmp[ii])
- + ((unsigned short)0x8000)));
- }
- } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) {
- tmp[ii] = (long)((signed char)raw_data[ii]) * 256;
- } else {
- result = INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- }
- }
-
- for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
- data[ii] = ((long)tmp[0] * mtx[3 * ii] +
- (long)tmp[1] * mtx[3 * ii + 1] +
- (long)tmp[2] * mtx[3 * ii + 2]);
- }
-
- //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]);
- return result;
-}
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/accel.h b/60xx/mlsdk/mllite/accel.h
deleted file mode 100644
index d3fbc6a..0000000
--- a/60xx/mlsdk/mllite/accel.h
+++ /dev/null
@@ -1,57 +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: accel.h 4580 2011-01-22 03:19:23Z prao $
- *
- *******************************************************************************/
-
-#ifndef ACCEL_H
-#define ACCEL_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "accel_legacy.h"
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-
- 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
-}
-#endif
-#endif // ACCEL_H
diff --git a/60xx/mlsdk/mllite/compass.c b/60xx/mlsdk/mllite/compass.c
deleted file mode 100644
index 798cb9f..0000000
--- a/60xx/mlsdk/mllite/compass.c
+++ /dev/null
@@ -1,579 +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: compass.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- * @defgroup COMPASSDL
- * @brief Motion Library - Compass Driver Layer.
- * Provides the interface to setup and handle an compass
- * connected to either the primary or the seconday I2C interface
- * of the gyroscope.
- *
- * @{
- * @file compass.c
- * @brief Compass setup and handling methods.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-#include <stdlib.h>
-#include "compass.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-compass"
-
-#define COMPASS_DEBUG 0
-
-/* --------------------- */
-/* - Global Variables. - */
-/* --------------------- */
-
-/* --------------------- */
-/* - Static Variables. - */
-/* --------------------- */
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* -------------- */
-/* - Externs. - */
-/* -------------- */
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-static float square(float data)
-{
- return data * data;
-}
-
-static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise)
-{
- int i;
-
- adap_filter->num = 0;
- adap_filter->index = 0;
- adap_filter->noise = noise;
- adap_filter->len = len;
-
- for (i = 0; i < adap_filter->len; ++i) {
- adap_filter->sequence[i] = 0;
- }
-}
-
-static int cmpfloat(const void *p1, const void *p2)
-{
- return *(float*)p1 - *(float*)p2;
-}
-
-
-static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in)
-{
- float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN];
- int i;
-
- if (adap_filter->len <= 1) {
- return in;
- }
- if (adap_filter->num < adap_filter->len) {
- adap_filter->sequence[adap_filter->index++] = in;
- adap_filter->num++;
- return in;
- }
- if (adap_filter->len <= adap_filter->index) {
- adap_filter->index = 0;
- }
- adap_filter->sequence[adap_filter->index++] = in;
-
- avg = 0;
- for (i = 0; i < adap_filter->len; i++) {
- avg += adap_filter->sequence[i];
- }
- avg /= adap_filter->len;
-
- memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float));
- qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat);
- median = sorted[adap_filter->len/2];
-
- sum = 0;
- for (i = 0; i < adap_filter->len; i++) {
- sum += square(avg - adap_filter->sequence[i]);
- }
- sum /= adap_filter->len;
-
- if (sum <= adap_filter->noise) {
- return median;
- }
-
- return ((in - avg) * (sum - adap_filter->noise) / sum + avg);
-}
-
-static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold)
-{
- thresh_filter->threshold = threshold;
- thresh_filter->last = 0;
-}
-
-static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in)
-{
- if (in < thresh_filter->last - thresh_filter->threshold
- || thresh_filter->last + thresh_filter->threshold < in) {
- thresh_filter->last = in;
- return in;
- }
- else {
- return thresh_filter->last;
- }
-}
-
-static int init(yas_filter_handle_t *t)
-{
- float noise[] = {
- YAS_DEFAULT_FILTER_NOISE,
- YAS_DEFAULT_FILTER_NOISE,
- YAS_DEFAULT_FILTER_NOISE,
- };
- int i;
-
- if (t == NULL) {
- return -1;
- }
-
- for (i = 0; i < 3; i++) {
- adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]);
- thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH);
- }
-
- return 0;
-}
-
-static int update(yas_filter_handle_t *t, float *input, float *output)
-{
- int i;
-
- if (t == NULL || input == NULL || output == NULL) {
- return -1;
- }
-
- for (i = 0; i < 3; i++) {
- output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]);
- output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]);
- }
-
- return 0;
-}
-
-int yas_filter_init(yas_filter_if_s *f)
-{
- if (f == NULL) {
- return -1;
- }
- f->init = init;
- f->update = update;
-
- return 0;
-}
-
-/**
- * @brief Used to determine if a compass is
- * configured and used by the MPL.
- * @return INV_SUCCESS if the compass is present.
- */
-unsigned char inv_compass_present(void)
-{
- INVENSENSE_FUNC_START;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- if (NULL != mldl_cfg->compass &&
- NULL != mldl_cfg->compass->resume &&
- mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
- return TRUE;
- else
- return FALSE;
-}
-
-/**
- * @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
- * X, Y, and Z axes.
- * @return INV_SUCCESS or a non-zero error code.
- */
-inv_error_t inv_get_compass_data(long *data)
-{
- inv_error_t result;
- int ii;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- unsigned char *tmp = inv_obj.compass_raw_data;
-
- if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
- return INV_ERROR_INVALID_CONFIGURATION;
- }
-
- if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY ||
- !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
- /*--- read the compass sensor data.
- The compass read function may return an INV_ERROR_COMPASS_* 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 compass data sample. ---*/
- result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(),
- tmp);
- if (result) {
- if (COMPASS_DEBUG) {
- MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
- }
- return result;
- }
- for (ii = 0; ii < 3; ii++) {
- if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian)
- data[ii] =
- ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
- else
- data[ii] =
- ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
- }
-
- inv_obj.compass_overunder = (int)tmp[6];
-
- } else {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- result = inv_get_external_sensor_data(data, 3);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
- {
- static unsigned char first = TRUE;
- // one-off write to AKM
- if (first) {
- unsigned char regs[] = {
- // beginning Mantis register for one-off slave R/W
- MPUREG_I2C_SLV4_ADDR,
- // the slave to write to
- mldl_cfg->pdata->compass.address,
- // the register to write to
- /*mldl_cfg->compass->trigger->reg */ 0x0A,
- // the value to write
- /*mldl_cfg->compass->trigger->value */ 0x01,
- // enable the write
- 0xC0
- };
- result =
- inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
- ARRAY_SIZE(regs), regs);
- first = FALSE;
- } else {
- unsigned char regs[] = {
- MPUREG_I2C_SLV4_CTRL,
- 0xC0
- };
- result =
- inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
- ARRAY_SIZE(regs), regs);
- }
- }
-#endif
-#else
- return INV_ERROR_INVALID_CONFIGURATION;
-#endif // CONFIG_MPU_SENSORS_xxxx
- }
- data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]);
- data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]);
- data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]);
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Sets the compass bias.
- * @param bias
- * Compass bias, length 3. Scale is micro Tesla's * 2^16.
- * Frame is mount frame which may be different from body frame.
- * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
- */
-inv_error_t inv_set_compass_bias(long *bias)
-{
- inv_error_t result = INV_SUCCESS;
- long biasC[3];
- struct mldl_cfg *mldlCfg = inv_get_dl_config();
-
- inv_obj.compass_bias[0] = bias[0];
- inv_obj.compass_bias[1] = bias[1];
- inv_obj.compass_bias[2] = bias[2];
-
- // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
- biasC[0] =
- (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
- inv_obj.init_compass_bias[0] * (1L << 16);
- biasC[1] =
- (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
- inv_obj.init_compass_bias[1] * (1L << 16);
- biasC[2] =
- (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
- inv_obj.init_compass_bias[2] * (1L << 16);
-
- if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
- unsigned char reg[4];
- long biasB[3];
- signed char *orC = mldlCfg->pdata->compass.orientation;
-
- // Now transform to body frame
- biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
- biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
- biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
-
- result =
- inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
- inv_int32_to_big8(biasB[0], reg));
- result =
- inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
- inv_int32_to_big8(biasB[1], reg));
- result =
- inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
- inv_int32_to_big8(biasB[2], reg));
- }
- return result;
-}
-
-/**
- * @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
- * 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_scale(long *val)
-{
- struct ext_slave_config config;
- unsigned char data[3];
- inv_error_t result;
-
- config.key = MPU_SLAVE_READ_SCALE;
- config.len = 3;
- 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[0] = ((data[0] - 128) << 22) + (1L << 30);
- val[1] = ((data[1] - 128) << 22) + (1L << 30);
- val[2] = ((data[2] - 128) << 22) + (1L << 30);
- return result;
-}
-
-inv_error_t inv_set_compass_offset(void)
-{
- struct ext_slave_config config;
- unsigned char data[3];
- inv_error_t result;
-
- config.key = MPU_SLAVE_OFFSET_VALS;
- config.len = 3;
- config.apply = TRUE;
- config.data = data;
-
- if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
- /* push stored values */
- data[0] = (char)inv_obj.compass_offsets[0];
- data[1] = (char)inv_obj.compass_offsets[1];
- data[2] = (char)inv_obj.compass_offsets[2];
- MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
- result = inv_mpu_config_compass(inv_get_dl_config(),
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- } else {
- /* compute new values and store them */
- result = inv_mpu_get_compass_config(inv_get_dl_config(),
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
- if(result == INV_SUCCESS) {
- inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
- inv_obj.compass_offsets[0] = data[0];
- inv_obj.compass_offsets[1] = data[1];
- inv_obj.compass_offsets[2] = data[2];
- }
- }
-
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-inv_error_t inv_compass_check_range(void)
-{
- struct ext_slave_config config;
- unsigned char data[3];
- inv_error_t result;
-
- config.key = MPU_SLAVE_RANGE_CHECK;
- config.len = 3;
- 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;
- }
-
- if(data[0] || data[1] || data[2]) {
- /* some value clipped */
- return INV_ERROR_COMPASS_DATA_ERROR;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/compass.h b/60xx/mlsdk/mllite/compass.h
deleted file mode 100644
index f0bdb58..0000000
--- a/60xx/mlsdk/mllite/compass.h
+++ /dev/null
@@ -1,92 +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: compass.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef COMPASS_H
-#define COMPASS_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "compass_legacy.h"
-#endif
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
-#define YAS_MAX_FILTER_LEN (20)
-#define YAS_DEFAULT_FILTER_LEN (20)
-#define YAS_DEFAULT_FILTER_THRESH (300) /* 300 nT */
-#define YAS_DEFAULT_FILTER_NOISE (2000 * 2000) /* standard deviation 2000 nT */
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
-struct yas_adaptive_filter {
- int num;
- int index;
- int len;
- float noise;
- float sequence[YAS_MAX_FILTER_LEN];
-};
-
-struct yas_thresh_filter {
- float threshold;
- float last;
-};
-
-typedef struct {
- struct yas_adaptive_filter adap_filter[3];
- struct yas_thresh_filter thresh_filter[3];
-} yas_filter_handle_t;
-
-typedef struct {
- int (*init)(yas_filter_handle_t *t);
- int (*update)(yas_filter_handle_t *t, float *input, float *output);
-} yas_filter_if_s;
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-
- 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);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // COMPASS_H
diff --git a/60xx/mlsdk/mllite/dmpDefault.c b/60xx/mlsdk/mllite/dmpDefault.c
deleted file mode 100644
index 6620d14..0000000
--- a/60xx/mlsdk/mllite/dmpDefault.c
+++ /dev/null
@@ -1,417 +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: dmpDefault.c 5627 2011-06-10 22:34:18Z nroyer $
- ******************************************************************************/
-
-/* WARNING: autogenerated code, do not modify */
-/**
- * @defgroup DMPDEFAULT
- * @brief Data and configuration for MLDmpDefaultOpen.
- *
- * @{
- * @file inv_setup_dmp.c
- * @brief Data and configuration for MLDmpDefaultOpen.
- */
-
-#include "mltypes.h"
-#include "dmpDefault.h"
-#include "dmpKey.h"
-#include "dmpmap.h"
-#include "ml.h"
-#include "mpu.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-
-#define CFG_25 703
-#define CFG_24 699
-#define CFG_26 707
-#define CFG_21 802
-#define CFG_20 645
-#define CFG_23 814
-#define CFG_TAP4 808
-#define CFG_TAP5 809
-#define CFG_TAP6 810
-#define CFG_1 783
-#define CFG_TAP0 802
-#define CFG_TAP1 804
-#define CFG_TAP2 805
-#define CFG_TAP3 806
-#define FCFG_AZ 878
-#define CFG_ORIENT_IRQ_1 715
-#define CFG_ORIENT_IRQ_2 738
-#define CFG_ORIENT_IRQ_3 743
-#define CFG_TAP_QUANTIZE 647
-#define FCFG_3 936
-#define CFG_TAP_CLEAR_STICKY 817
-#define FCFG_1 868
-#define CFG_ACCEL_FILTER 968
-#define FCFG_2 872
-#define CFG_3D 521
-#define CFG_3B 517
-#define CFG_3C 519
-#define FCFG_5 942
-#define FCFG_4 857
-#define FCFG_FSCALE 877
-#define CFG_TAP_JERK 639
-#define FCFG_6 996
-#define CFG_12 797
-#define FCFG_7 930
-#define CFG_14 790
-#define CFG_15 790
-#define CFG_16 815
-#define CFG_18 551
-#define CFG_6 823
-#define CFG_7 564
-#define CFG_4 526
-#define CFG_5 749
-#define CFG_3 515
-#define CFG_GYRO_SOURCE 777
-#define CFG_8 772
-#define CFG_9 778
-#define CFG_ORIENT_2 733
-#define CFG_ORIENT_1 713
-#define FCFG_ACCEL_INPUT 904
-#define CFG_TAP7 811
-#define CFG_TAP_SAVE_ACCB 687
-#define FCFG_ACCEL_INIT 831
-
-
-#define D_0_22 (22)
-#define D_0_24 (24)
-#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)
-
-
-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_TAP3, CFG_TAP3},
- {KEY_FCFG_AZ, FCFG_AZ},
- {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_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE},
- {KEY_FCFG_3, FCFG_3},
- {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY},
- {KEY_FCFG_1, FCFG_1},
- //{KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER},
- {KEY_FCFG_2, FCFG_2},
- {KEY_CFG_3D, CFG_3D},
- {KEY_CFG_3B, CFG_3B},
- {KEY_CFG_3C, CFG_3C},
- {KEY_FCFG_5, FCFG_5},
- {KEY_FCFG_4, FCFG_4},
- {KEY_FCFG_FSCALE, FCFG_FSCALE},
- {KEY_CFG_TAP_JERK, CFG_TAP_JERK},
- {KEY_FCFG_6, FCFG_6},
- {KEY_CFG_12, CFG_12},
- {KEY_FCFG_7, FCFG_7},
- {KEY_CFG_14, CFG_14},
- {KEY_CFG_15, CFG_15},
- {KEY_CFG_16, CFG_16},
- {KEY_CFG_18, CFG_18},
- {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_8, CFG_8},
- {KEY_CFG_9, CFG_9},
- {KEY_CFG_ORIENT_2, CFG_ORIENT_2},
- {KEY_CFG_ORIENT_1, CFG_ORIENT_1},
- {KEY_FCFG_ACCEL_INPUT, FCFG_ACCEL_INPUT},
- {KEY_CFG_TAP7, CFG_TAP7},
- {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB},
- {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_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}
-};
-
-#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0]))
-static const unsigned short sConfig = 0x013f;
-#define SCD (1024)
-static const unsigned char dmpMemory[SCD] = {
- 0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x5a, 0xd6, 0x96, 0x06, 0x3f, 0xa3, 0x00, 0x00,
- 0x20, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x77, 0x8e, 0x00, 0x01, 0x00, 0x01,
- 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01,
- 0x02, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x03, 0x06, 0x00, 0x00, 0x05, 0x01, 0xe9, 0xa2, 0x0f,
- 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
- 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x3e, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f,
- 0xfc, 0xba, 0xfa, 0x00, 0x01, 0x00, 0x80, 0x00, 0x02, 0x01, 0x80, 0x00, 0x03, 0x02, 0x80, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f,
- 0x00, 0x7d, 0x32, 0xba, 0x00, 0x0a, 0x1e, 0xd1, 0x00, 0x3a, 0xe8, 0x25, 0x00, 0x00, 0x00, 0x00,
- 0x3f, 0xd7, 0x96, 0x08, 0xff, 0xb3, 0x39, 0xf5, 0xfe, 0x11, 0x1b, 0x62, 0xfb, 0xf4, 0xb4, 0x52,
- 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,
- 0x0d, 0x68, 0x00, 0x00, 0x40, 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,
- 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x12, 0x82, 0x2d, 0x90,
- 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0xff, 0xff, 0x00, 0x05, 0x02, 0x00, 0x00, 0x0c,
- 0x00, 0x03, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 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, 0x20, 0x00, 0xff, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00,
- 0xff, 0xec, 0x3f, 0xc8, 0xff, 0xee, 0x00, 0x00, 0xff, 0xfe, 0x40, 0x00, 0xff, 0xff, 0xff, 0xc8,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0xff, 0xff, 0xff, 0xff, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 0x03, 0x65, 0x00, 0x00, 0x00, 0x99, 0x00, 0x00, 0x02, 0xf5,
-
- 0x9e, 0xc5, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, 0xc1, 0x83, 0x06, 0x26,
- 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xc4, 0xad, 0x00, 0x2c, 0x54, 0x7c, 0xf9, 0xc5, 0xa3,
- 0xc1, 0xc3, 0x8f, 0x96, 0x19, 0xa6, 0x81, 0xda, 0x0c, 0xd9, 0x2e, 0xd8, 0xa3, 0x86, 0x31, 0x81,
- 0xa6, 0xd9, 0x30, 0x26, 0xd8, 0xd8, 0xfa, 0xc1, 0x8c, 0xc2, 0x99, 0xc5, 0xa3, 0x2d, 0x55, 0x7d,
- 0x81, 0x91, 0xac, 0x38, 0xad, 0x3a, 0xc3, 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, 0xfb, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81,
- 0xad, 0xd9, 0x01, 0xd8, 0xfa, 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad,
- 0xad, 0xad, 0xad, 0xfb, 0x2a, 0xd8, 0xd8, 0xf9, 0xc0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xfb,
- 0xac, 0x2e, 0x2e, 0xf9, 0xc1, 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, 0xfa,
- 0xc0, 0x89, 0xac, 0x91, 0x2c, 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8,
- 0xd8, 0x79, 0xd9, 0xd8, 0xd8, 0xf9, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9,
- 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xc1, 0x83, 0x93, 0x35, 0x3d, 0x80,
- 0x25, 0xda, 0xd8, 0xd8, 0x85, 0x69, 0xda, 0xd8, 0xd8, 0xf9, 0xc2, 0x93, 0x81, 0xa3, 0x28, 0x34,
- 0x3c, 0xfb, 0x91, 0xab, 0x8b, 0x18, 0xa3, 0x09, 0xd9, 0xab, 0x97, 0x0a, 0x91, 0x3c, 0xc0, 0x87,
-
- 0x9c, 0xc5, 0xa3, 0xdd, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x95, 0xf9, 0xa3, 0xa3, 0xa3, 0x9d, 0xf9,
- 0xa3, 0xa3, 0xa3, 0xa3, 0xf9, 0x90, 0xa3, 0xa3, 0xa3, 0xa3, 0x91, 0xc3, 0x99, 0xf9, 0xa3, 0xa3,
- 0xa3, 0x98, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xfb, 0x9b, 0xa3, 0xa3,
- 0xdc, 0xc5, 0xa7, 0xf9, 0x26, 0x26, 0x26, 0xd8, 0xd8, 0xff, 0xd8, 0xd8, 0xd8, 0xd8, 0xd8, 0xc1,
- 0xc2, 0xc4, 0x81, 0xa0, 0x90, 0xfa, 0x2c, 0x80, 0x74, 0xfb, 0x70, 0xfa, 0x7c, 0xc0, 0x86, 0x98,
- 0xa8, 0xf9, 0xc9, 0x88, 0xa1, 0xfa, 0x0e, 0x97, 0x80, 0xf9, 0xa9, 0x2e, 0x2e, 0x2e, 0xaa, 0x2e,
- 0x2e, 0x2e, 0xfa, 0xaa, 0xc9, 0x2c, 0xcb, 0xa9, 0x4c, 0xcd, 0x6c, 0xf9, 0x89, 0xa5, 0xca, 0xcd,
- 0xcf, 0xc3, 0x9e, 0xa9, 0x3e, 0x5e, 0x7e, 0x85, 0xa5, 0x1a, 0x3e, 0x5e, 0xc2, 0xa5, 0x99, 0xfb,
- 0x08, 0x34, 0x5c, 0xf9, 0xa9, 0xc9, 0xcb, 0xcd, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97,
- 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0xa9,
- 0xf9, 0x89, 0x26, 0x46, 0x66, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xaa, 0x98, 0x82, 0x87, 0x2d,
- 0x35, 0x3d, 0xc5, 0xa3, 0xc2, 0xc1, 0x97, 0x80, 0x4a, 0x4e, 0x4e, 0xa3, 0xfa, 0x48, 0xcd, 0xc9,
- 0xf9, 0xc4, 0xa9, 0x99, 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, 0xc3, 0x93,
- 0xa3, 0x0e, 0x16, 0x1e, 0xa9, 0x2c, 0x54, 0x7c, 0xc0, 0xc2, 0x83, 0x97, 0xaf, 0x08, 0xc4, 0xa8,
- 0x11, 0xc1, 0x8f, 0xc5, 0xaf, 0x98, 0xf8, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf9, 0xa3, 0x29,
- 0x55, 0x7d, 0xaf, 0x83, 0xc3, 0x93, 0xaf, 0xf8, 0x00, 0x28, 0x50, 0xc4, 0xc2, 0xc0, 0xf9, 0x97,
-};
-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, SCD, sConfig);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_ignore_system_suspend(FALSE);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- if (inv_accel_present())
- {
- struct ext_slave_config config;
- long odr;
- config.key = MPU_SLAVE_CONFIG_ODR_SUSPEND;
- config.len = sizeof(long);
- config.apply = FALSE;
- config.data = &odr;
-
- odr = 0;
- result = inv_mpu_config_accel(inv_get_dl_config(),
- inv_get_serial_handle(),
- inv_get_serial_handle(),
- &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
- odr = 200000;
- result = inv_mpu_config_accel(inv_get_dl_config(),
- inv_get_serial_handle(),
- inv_get_serial_handle(),
- &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- config.key = MPU_SLAVE_CONFIG_IRQ_SUSPEND;
- odr = MPU_SLAVE_IRQ_TYPE_NONE;
- result = inv_mpu_config_accel(inv_get_dl_config(),
- inv_get_serial_handle(),
- inv_get_serial_handle(),
- &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
- odr = MPU_SLAVE_IRQ_TYPE_NONE;
- result = inv_mpu_config_accel(inv_get_dl_config(),
- inv_get_serial_handle(),
- inv_get_serial_handle(),
- &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- }
-
- return result;
-}
-/**
- * @}
- */
-
diff --git a/60xx/mlsdk/mllite/dmpDefault.h b/60xx/mlsdk/mllite/dmpDefault.h
deleted file mode 100644
index 0670977..0000000
--- a/60xx/mlsdk/mllite/dmpDefault.h
+++ /dev/null
@@ -1,42 +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.
- $
- */
-#ifndef ML_DMPDEFAULT_H__
-#define ML_DMPDEFAULT_H__
-
-/**
- * @defgroup DEFAULT
- * @brief Default DMP assembly listing header file
- *
- * @{
- * @file inv_setup_dmp.c
- * @brief Default DMP assembly listing header file
- */
-
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
- inv_error_t inv_setup_dmp(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // ML_DMPDEFAULT_H__
diff --git a/60xx/mlsdk/mllite/dmpDefaultMantis.c b/60xx/mlsdk/mllite/dmpDefaultMantis.c
deleted file mode 100644
index f35aaca..0000000
--- a/60xx/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/60xx/mlsdk/mllite/dmpKey.h b/60xx/mlsdk/mllite/dmpKey.h
deleted file mode 100644
index f152644..0000000
--- a/60xx/mlsdk/mllite/dmpKey.h
+++ /dev/null
@@ -1,432 +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.
- $
- */
-#ifndef DMPKEY_H__
-#define DMPKEY_H__
-
-#define KEY_CFG_25 0
-#define KEY_CFG_24 (KEY_CFG_25+1)
-#define KEY_CFG_26 (KEY_CFG_24+1)
-#define KEY_CFG_21 (KEY_CFG_26+1)
-#define KEY_CFG_20 (KEY_CFG_21+1)
-#define KEY_CFG_TAP4 (KEY_CFG_20+1)
-#define KEY_CFG_TAP5 (KEY_CFG_TAP4+1)
-#define KEY_CFG_TAP6 (KEY_CFG_TAP5+1)
-#define KEY_CFG_TAP7 (KEY_CFG_TAP6+1)
-#define KEY_CFG_TAP0 (KEY_CFG_TAP7+1)
-#define KEY_CFG_TAP1 (KEY_CFG_TAP0+1)
-#define KEY_CFG_TAP2 (KEY_CFG_TAP1+1)
-#define KEY_CFG_TAP3 (KEY_CFG_TAP2+1)
-#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3+1)
-#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE+1)
-#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_TAP_JERK+1)
-#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB+1)
-#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_TAP_CLEAR_STICKY +1)
-#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT+1)
-#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT+1)
-#define KEY_FCFG_1 (KEY_CFG_23+1)
-#define KEY_FCFG_3 (KEY_FCFG_1+1)
-#define KEY_FCFG_2 (KEY_FCFG_3+1)
-#define KEY_CFG_3D (KEY_FCFG_2+1)
-#define KEY_CFG_3B (KEY_CFG_3D+1)
-#define KEY_CFG_3C (KEY_CFG_3B+1)
-#define KEY_FCFG_5 (KEY_CFG_3C+1)
-#define KEY_FCFG_4 (KEY_FCFG_5+1)
-#define KEY_FCFG_7 (KEY_FCFG_4+1)
-#define KEY_FCFG_FSCALE (KEY_FCFG_7+1)
-#define KEY_FCFG_AZ (KEY_FCFG_FSCALE+1)
-#define KEY_FCFG_6 (KEY_FCFG_AZ+1)
-#define KEY_FCFG_LSB4 (KEY_FCFG_6+1)
-#define KEY_CFG_12 (KEY_FCFG_LSB4+1)
-#define KEY_CFG_14 (KEY_CFG_12+1)
-#define KEY_CFG_15 (KEY_CFG_14+1)
-#define KEY_CFG_16 (KEY_CFG_15+1)
-#define KEY_CFG_18 (KEY_CFG_16+1)
-#define KEY_CFG_6 (KEY_CFG_18 + 1)
-#define KEY_CFG_7 (KEY_CFG_6+1)
-#define KEY_CFG_4 (KEY_CFG_7+1)
-#define KEY_CFG_5 (KEY_CFG_4+1)
-#define KEY_CFG_2 (KEY_CFG_5+1)
-#define KEY_CFG_3 (KEY_CFG_2+1)
-#define KEY_CFG_1 (KEY_CFG_3+1)
-#define KEY_CFG_EXTERNAL (KEY_CFG_1+1)
-#define KEY_CFG_8 (KEY_CFG_EXTERNAL+1)
-#define KEY_CFG_9 (KEY_CFG_8+1)
-#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1)
-#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1)
-#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1)
-#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1)
-#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1)
-#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1)
-#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1)
-#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1)
-#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1)
-
-#define KEY_D_0_22 (KEY_FCFG_MAG_MOV + 1)
-#define KEY_D_0_24 (KEY_D_0_22+1)
-#define KEY_D_0_36 (KEY_D_0_24+1)
-#define KEY_D_0_52 (KEY_D_0_36+1)
-#define KEY_D_0_96 (KEY_D_0_52+1)
-#define KEY_D_0_104 (KEY_D_0_96+1)
-#define KEY_D_0_108 (KEY_D_0_104+1)
-#define KEY_D_0_163 (KEY_D_0_108+1)
-#define KEY_D_0_188 (KEY_D_0_163+1)
-#define KEY_D_0_192 (KEY_D_0_188+1)
-#define KEY_D_0_224 (KEY_D_0_192+1)
-#define KEY_D_0_228 (KEY_D_0_224+1)
-#define KEY_D_0_232 (KEY_D_0_228+1)
-#define KEY_D_0_236 (KEY_D_0_232+1)
-
-#define KEY_DMP_PREVPTAT (KEY_D_0_236+1)
-#define KEY_D_1_2 (KEY_DMP_PREVPTAT+1)
-#define KEY_D_1_4 (KEY_D_1_2 + 1)
-#define KEY_D_1_8 (KEY_D_1_4 + 1)
-#define KEY_D_1_10 (KEY_D_1_8+1)
-#define KEY_D_1_24 (KEY_D_1_10+1)
-#define KEY_D_1_28 (KEY_D_1_24+1)
-#define KEY_D_1_92 (KEY_D_1_28+1)
-#define KEY_D_1_96 (KEY_D_1_92+1)
-#define KEY_D_1_98 (KEY_D_1_96+1)
-#define KEY_D_1_106 (KEY_D_1_98+1)
-#define KEY_D_1_108 (KEY_D_1_106+1)
-#define KEY_D_1_112 (KEY_D_1_108+1)
-#define KEY_D_1_128 (KEY_D_1_112+1)
-#define KEY_D_1_152 (KEY_D_1_128+1)
-#define KEY_D_1_168 (KEY_D_1_152+1)
-#define KEY_D_1_175 (KEY_D_1_168+1)
-#define KEY_D_1_178 (KEY_D_1_175+1)
-#define KEY_D_1_179 (KEY_D_1_178+1)
-#define KEY_D_1_236 (KEY_D_1_179+1)
-#define KEY_D_1_244 (KEY_D_1_236+1)
-#define KEY_D_2_12 (KEY_D_1_244+1)
-#define KEY_D_2_96 (KEY_D_2_12+1)
-#define KEY_D_2_108 (KEY_D_2_96+1)
-#define KEY_D_2_244 (KEY_D_2_108+1)
-#define KEY_D_2_248 (KEY_D_2_244+1)
-#define KEY_D_2_252 (KEY_D_2_248+1)
-
-// Compass Keys
-#define KEY_CPASS_BIAS_X (KEY_D_2_252+1)
-#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X+1)
-#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y+1)
-#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z+1)
-#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00+1)
-#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01+1)
-#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02+1)
-#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10+1)
-#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11+1)
-#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12+1)
-#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20+1)
-#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21+1)
-
-// Mantis Keys
-#define KEY_CFG_MOTION_BIAS (KEY_CPASS_MTX_22+1)
-
-#define KEY_DMP_TAPW_MIN (KEY_CFG_MOTION_BIAS+1)
-#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN+1)
-#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X+1)
-#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y+1)
-#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z+1)
-#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y+1)
-#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X+1)
-#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z+1)
-#define KEY_D_ACT0 (KEY_DMP_ORIENT+1)
-#define KEY_D_ACSX (KEY_D_ACT0+1)
-#define KEY_D_ACSY (KEY_D_ACSX+1)
-#define KEY_D_ACSZ (KEY_D_ACSY+1)
-
-// Pedometer Standalone only keys
-#define KEY_D_PEDSTD_BP_B (KEY_D_ACSZ+1)
-#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B+1)
-#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A+1)
-#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B+1)
-#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4+1)
-#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3+1)
-#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2+1)
-#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1+1)
-#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH+1)
-#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP+1)
-#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB+1)
-#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME+1)
-#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH+1)
-#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML+1)
-#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH+1)
-#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK+1)
-#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR+1)
-#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR+1)
-
-// EIS Keys
-#define KEY_P_EIS_FIFO_FOOTER (KEY_D_PEDSTD_WALKTIME+1)
-#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER+1)
-#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT+1)
-#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE+1)
-#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT+1)
-#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC+1)
-#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT+1)
-#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY+1)
-#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER+1)
-#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC+1)
-#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH+1)
-#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH +1)
-#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH+1)
-#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH+1)
-#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH+1)
-#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H+1)
-#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H+1)
-#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H+1)
-#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H+1)
-#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H+1)
-#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H+1)
-#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH+1)
-#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH+1)
-#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH+1)
-#define KEY_DMP_INTY_H (KEY_DMP_INTX_H+1)
-#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H+1)
-#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H+1)
-#define KEY_DMP_HPY_H (KEY_DMP_HPX_H+1)
-#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H+1)
-
-// Stream Keys
-#define KEY_STREAM_P_GYRO_Z (KEY_DMP_HPZ_H + 1)
-#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z+1)
-#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y+1)
-#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X+1)
-#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP+1)
-#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y+1)
-#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X+1)
-#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z+1)
-#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y+1)
-#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X+1)
-#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER+1)
-
-#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z+1)
-
- typedef struct {
- unsigned short key;
- unsigned short addr;
- } tKeyLabel;
-
-#define DINA0A 0x0a
-#define DINA22 0x22
-#define DINA42 0x42
-#define DINA5A 0x5a
-
-#define DINA06 0x06
-#define DINA0E 0x0e
-#define DINA16 0x16
-#define DINA1E 0x1e
-#define DINA26 0x26
-#define DINA2E 0x2e
-#define DINA36 0x36
-#define DINA3E 0x3e
-#define DINA46 0x46
-#define DINA4E 0x4e
-#define DINA56 0x56
-#define DINA5E 0x5e
-#define DINA66 0x66
-#define DINA6E 0x6e
-#define DINA76 0x76
-#define DINA7E 0x7e
-
-#define DINA00 0x00
-#define DINA08 0x08
-#define DINA10 0x10
-#define DINA18 0x18
-#define DINA20 0x20
-#define DINA28 0x28
-#define DINA30 0x30
-#define DINA38 0x38
-#define DINA40 0x40
-#define DINA48 0x48
-#define DINA50 0x50
-#define DINA58 0x58
-#define DINA60 0x60
-#define DINA68 0x68
-#define DINA70 0x70
-#define DINA78 0x78
-
-#define DINA04 0x04
-#define DINA0C 0x0c
-#define DINA14 0x14
-#define DINA1C 0x1C
-#define DINA24 0x24
-#define DINA2C 0x2c
-#define DINA34 0x34
-#define DINA3C 0x3c
-#define DINA44 0x44
-#define DINA4C 0x4c
-#define DINA54 0x54
-#define DINA5C 0x5c
-#define DINA64 0x64
-#define DINA6C 0x6c
-#define DINA74 0x74
-#define DINA7C 0x7c
-
-#define DINA01 0x01
-#define DINA09 0x09
-#define DINA11 0x11
-#define DINA19 0x19
-#define DINA21 0x21
-#define DINA29 0x29
-#define DINA31 0x31
-#define DINA39 0x39
-#define DINA41 0x41
-#define DINA49 0x49
-#define DINA51 0x51
-#define DINA59 0x59
-#define DINA61 0x61
-#define DINA69 0x69
-#define DINA71 0x71
-#define DINA79 0x79
-
-#define DINA25 0x25
-#define DINA2D 0x2d
-#define DINA35 0x35
-#define DINA3D 0x3d
-#define DINA4D 0x4d
-#define DINA55 0x55
-#define DINA5D 0x5D
-#define DINA6D 0x6d
-#define DINA75 0x75
-#define DINA7D 0x7d
-
-#define DINC00 0x00
-#define DINC01 0x01
-#define DINC02 0x02
-#define DINC03 0x03
-#define DINC08 0x08
-#define DINC09 0x09
-#define DINC0A 0x0a
-#define DINC0B 0x0b
-#define DINC10 0x10
-#define DINC11 0x11
-#define DINC12 0x12
-#define DINC13 0x13
-#define DINC18 0x18
-#define DINC19 0x19
-#define DINC1A 0x1a
-#define DINC1B 0x1b
-
-#define DINC20 0x20
-#define DINC21 0x21
-#define DINC22 0x22
-#define DINC23 0x23
-#define DINC28 0x28
-#define DINC29 0x29
-#define DINC2A 0x2a
-#define DINC2B 0x2b
-#define DINC30 0x30
-#define DINC31 0x31
-#define DINC32 0x32
-#define DINC33 0x33
-#define DINC38 0x38
-#define DINC39 0x39
-#define DINC3A 0x3a
-#define DINC3B 0x3b
-
-#define DINC40 0x40
-#define DINC41 0x41
-#define DINC42 0x42
-#define DINC43 0x43
-#define DINC48 0x48
-#define DINC49 0x49
-#define DINC4A 0x4a
-#define DINC4B 0x4b
-#define DINC50 0x50
-#define DINC51 0x51
-#define DINC52 0x52
-#define DINC53 0x53
-#define DINC58 0x58
-#define DINC59 0x59
-#define DINC5A 0x5a
-#define DINC5B 0x5b
-
-#define DINC60 0x60
-#define DINC61 0x61
-#define DINC62 0x62
-#define DINC63 0x63
-#define DINC68 0x68
-#define DINC69 0x69
-#define DINC6A 0x6a
-#define DINC6B 0x6b
-#define DINC70 0x70
-#define DINC71 0x71
-#define DINC72 0x72
-#define DINC73 0x73
-#define DINC78 0x78
-#define DINC79 0x79
-#define DINC7A 0x7a
-#define DINC7B 0x7b
-
-#if defined CONFIG_MPU_SENSORS_MPU3050
-#define DINA80 0x80
-#define DINA90 0x90
-#define DINAA0 0xa0
-#define DINAC9 0xc9
-#define DINACB 0xcb
-#define DINACD 0xcd
-#define DINACF 0xcf
-#define DINAC8 0xc8
-#define DINACA 0xca
-#define DINACC 0xcc
-#define DINACE 0xce
-#define DINAD8 0xd8
-#define DINADD 0xdd
-#define DINAF8 0xf8
-#define DINAFE 0xfe
-#define DINAC0 0xc0
-#define DINAC1 0xc1
-#define DINAC2 0xc2
-#define DINAC3 0xc3
-#define DINAC4 0xc4
-#define DINAC5 0xc5
-#elif defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
-
-#define DINA80 0x80
-#define DINA90 0x90
-#define DINAA0 0xa0
-#define DINAC9 0xc9
-#define DINACB 0xcb
-#define DINACD 0xcd
-#define DINACF 0xcf
-#define DINAC8 0xc8
-#define DINACA 0xca
-#define DINACC 0xcc
-#define DINACE 0xce
-#define DINAD8 0xd8
-#define DINADD 0xdd
-#define DINAF8 0xf0
-#define DINAFE 0xfe
-
-#define DINBF8 0xf8
-#define DINAC0 0xb0
-#define DINAC1 0xb1
-#define DINAC2 0xb4
-#define DINAC3 0xb5
-#define DINAC4 0xb8
-#define DINAC5 0xb9
-#define DINBC0 0xc0
-#define DINBC2 0xc2
-#define DINBC4 0xc4
-#define DINBC6 0xc6
-#else
-#error No CONFIG_MPU_SENSORS_xxxx has been defined.
-#endif
-
-
-#endif // DMPKEY_H__
diff --git a/60xx/mlsdk/mllite/dmpconfig.txt b/60xx/mlsdk/mllite/dmpconfig.txt
deleted file mode 100644
index 4643ed5..0000000
--- a/60xx/mlsdk/mllite/dmpconfig.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-major version = 1
-minor version = 0
-startAddr = 0
diff --git a/60xx/mlsdk/mllite/dmpmap.h b/60xx/mlsdk/mllite/dmpmap.h
deleted file mode 100644
index cb53c7c..0000000
--- a/60xx/mlsdk/mllite/dmpmap.h
+++ /dev/null
@@ -1,276 +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.
- $
- */
-#ifndef DMPMAP_H
-#define DMPMAP_H
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-#define DMP_PTAT 0
-#define DMP_XGYR 2
-#define DMP_YGYR 4
-#define DMP_ZGYR 6
-#define DMP_XACC 8
-#define DMP_YACC 10
-#define DMP_ZACC 12
-#define DMP_ADC1 14
-#define DMP_ADC2 16
-#define DMP_ADC3 18
-#define DMP_BIASUNC 20
-#define DMP_FIFORT 22
-#define DMP_INVGSFH 24
-#define DMP_INVGSFL 26
-#define DMP_1H 28
-#define DMP_1L 30
-#define DMP_BLPFSTCH 32
-#define DMP_BLPFSTCL 34
-#define DMP_BLPFSXH 36
-#define DMP_BLPFSXL 38
-#define DMP_BLPFSYH 40
-#define DMP_BLPFSYL 42
-#define DMP_BLPFSZH 44
-#define DMP_BLPFSZL 46
-#define DMP_BLPFMTC 48
-#define DMP_SMC 50
-#define DMP_BLPFMXH 52
-#define DMP_BLPFMXL 54
-#define DMP_BLPFMYH 56
-#define DMP_BLPFMYL 58
-#define DMP_BLPFMZH 60
-#define DMP_BLPFMZL 62
-#define DMP_BLPFC 64
-#define DMP_SMCTH 66
-#define DMP_0H2 68
-#define DMP_0L2 70
-#define DMP_BERR2H 72
-#define DMP_BERR2L 74
-#define DMP_BERR2NH 76
-#define DMP_SMCINC 78
-#define DMP_ANGVBXH 80
-#define DMP_ANGVBXL 82
-#define DMP_ANGVBYH 84
-#define DMP_ANGVBYL 86
-#define DMP_ANGVBZH 88
-#define DMP_ANGVBZL 90
-#define DMP_BERR1H 92
-#define DMP_BERR1L 94
-#define DMP_ATCH 96
-#define DMP_BIASUNCSF 98
-#define DMP_ACT2H 100
-#define DMP_ACT2L 102
-#define DMP_GSFH 104
-#define DMP_GSFL 106
-#define DMP_GH 108
-#define DMP_GL 110
-#define DMP_0_5H 112
-#define DMP_0_5L 114
-#define DMP_0_0H 116
-#define DMP_0_0L 118
-#define DMP_1_0H 120
-#define DMP_1_0L 122
-#define DMP_1_5H 124
-#define DMP_1_5L 126
-#define DMP_TMP1AH 128
-#define DMP_TMP1AL 130
-#define DMP_TMP2AH 132
-#define DMP_TMP2AL 134
-#define DMP_TMP3AH 136
-#define DMP_TMP3AL 138
-#define DMP_TMP4AH 140
-#define DMP_TMP4AL 142
-#define DMP_XACCW 144
-#define DMP_TMP5 146
-#define DMP_XACCB 148
-#define DMP_TMP8 150
-#define DMP_YACCB 152
-#define DMP_TMP9 154
-#define DMP_ZACCB 156
-#define DMP_TMP10 158
-#define DMP_DZH 160
-#define DMP_DZL 162
-#define DMP_XGCH 164
-#define DMP_XGCL 166
-#define DMP_YGCH 168
-#define DMP_YGCL 170
-#define DMP_ZGCH 172
-#define DMP_ZGCL 174
-#define DMP_YACCW 176
-#define DMP_TMP7 178
-#define DMP_AFB1H 180
-#define DMP_AFB1L 182
-#define DMP_AFB2H 184
-#define DMP_AFB2L 186
-#define DMP_MAGFBH 188
-#define DMP_MAGFBL 190
-#define DMP_QT1H 192
-#define DMP_QT1L 194
-#define DMP_QT2H 196
-#define DMP_QT2L 198
-#define DMP_QT3H 200
-#define DMP_QT3L 202
-#define DMP_QT4H 204
-#define DMP_QT4L 206
-#define DMP_CTRL1H 208
-#define DMP_CTRL1L 210
-#define DMP_CTRL2H 212
-#define DMP_CTRL2L 214
-#define DMP_CTRL3H 216
-#define DMP_CTRL3L 218
-#define DMP_CTRL4H 220
-#define DMP_CTRL4L 222
-#define DMP_CTRLS1 224
-#define DMP_CTRLSF1 226
-#define DMP_CTRLS2 228
-#define DMP_CTRLSF2 230
-#define DMP_CTRLS3 232
-#define DMP_CTRLSFNLL 234
-#define DMP_CTRLS4 236
-#define DMP_CTRLSFNL2 238
-#define DMP_CTRLSFNL 240
-#define DMP_TMP30 242
-#define DMP_CTRLSFJT 244
-#define DMP_TMP31 246
-#define DMP_TMP11 248
-#define DMP_CTRLSF2_2 250
-#define DMP_TMP12 252
-#define DMP_CTRLSF1_2 254
-#define DMP_PREVPTAT 256
-#define DMP_ACCZB 258
-#define DMP_ACCXB 264
-#define DMP_ACCYB 266
-#define DMP_1HB 272
-#define DMP_1LB 274
-#define DMP_0H 276
-#define DMP_0L 278
-#define DMP_ASR22H 280
-#define DMP_ASR22L 282
-#define DMP_ASR6H 284
-#define DMP_ASR6L 286
-#define DMP_TMP13 288
-#define DMP_TMP14 290
-#define DMP_FINTXH 292
-#define DMP_FINTXL 294
-#define DMP_FINTYH 296
-#define DMP_FINTYL 298
-#define DMP_FINTZH 300
-#define DMP_FINTZL 302
-#define DMP_TMP1BH 304
-#define DMP_TMP1BL 306
-#define DMP_TMP2BH 308
-#define DMP_TMP2BL 310
-#define DMP_TMP3BH 312
-#define DMP_TMP3BL 314
-#define DMP_TMP4BH 316
-#define DMP_TMP4BL 318
-#define DMP_STXG 320
-#define DMP_ZCTXG 322
-#define DMP_STYG 324
-#define DMP_ZCTYG 326
-#define DMP_STZG 328
-#define DMP_ZCTZG 330
-#define DMP_CTRLSFJT2 332
-#define DMP_CTRLSFJTCNT 334
-#define DMP_PVXG 336
-#define DMP_TMP15 338
-#define DMP_PVYG 340
-#define DMP_TMP16 342
-#define DMP_PVZG 344
-#define DMP_TMP17 346
-#define DMP_MNMFLAGH 352
-#define DMP_MNMFLAGL 354
-#define DMP_MNMTMH 356
-#define DMP_MNMTML 358
-#define DMP_MNMTMTHRH 360
-#define DMP_MNMTMTHRL 362
-#define DMP_MNMTHRH 364
-#define DMP_MNMTHRL 366
-#define DMP_ACCQD4H 368
-#define DMP_ACCQD4L 370
-#define DMP_ACCQD5H 372
-#define DMP_ACCQD5L 374
-#define DMP_ACCQD6H 376
-#define DMP_ACCQD6L 378
-#define DMP_ACCQD7H 380
-#define DMP_ACCQD7L 382
-#define DMP_ACCQD0H 384
-#define DMP_ACCQD0L 386
-#define DMP_ACCQD1H 388
-#define DMP_ACCQD1L 390
-#define DMP_ACCQD2H 392
-#define DMP_ACCQD2L 394
-#define DMP_ACCQD3H 396
-#define DMP_ACCQD3L 398
-#define DMP_XN2H 400
-#define DMP_XN2L 402
-#define DMP_XN1H 404
-#define DMP_XN1L 406
-#define DMP_YN2H 408
-#define DMP_YN2L 410
-#define DMP_YN1H 412
-#define DMP_YN1L 414
-#define DMP_YH 416
-#define DMP_YL 418
-#define DMP_B0H 420
-#define DMP_B0L 422
-#define DMP_A1H 424
-#define DMP_A1L 426
-#define DMP_A2H 428
-#define DMP_A2L 430
-#define DMP_SEM1 432
-#define DMP_FIFOCNT 434
-#define DMP_SH_TH_X 436
-#define DMP_PACKET 438
-#define DMP_SH_TH_Y 440
-#define DMP_FOOTER 442
-#define DMP_SH_TH_Z 444
-#define DMP_TEMP29 448
-#define DMP_TEMP30 450
-#define DMP_XACCB_PRE 452
-#define DMP_XACCB_PREL 454
-#define DMP_YACCB_PRE 456
-#define DMP_YACCB_PREL 458
-#define DMP_ZACCB_PRE 460
-#define DMP_ZACCB_PREL 462
-#define DMP_TMP22 464
-#define DMP_TAP_TIMER 466
-#define DMP_TAP_THX 468
-#define DMP_TAP_THY 472
-#define DMP_TAP_THZ 476
-#define DMP_TAPW_MIN 478
-#define DMP_TMP25 480
-#define DMP_TMP26 482
-#define DMP_TMP27 484
-#define DMP_TMP28 486
-#define DMP_ORIENT 488
-#define DMP_THRSH 490
-#define DMP_ENDIANH 492
-#define DMP_ENDIANL 494
-#define DMP_BLPFNMTCH 496
-#define DMP_BLPFNMTCL 498
-#define DMP_BLPFNMXH 500
-#define DMP_BLPFNMXL 502
-#define DMP_BLPFNMYH 504
-#define DMP_BLPFNMYL 506
-#define DMP_BLPFNMZH 508
-#define DMP_BLPFNMZL 510
-#ifdef __cplusplus
-}
-#endif
-#endif // DMPMAP_H
diff --git a/60xx/mlsdk/mllite/invensense.h b/60xx/mlsdk/mllite/invensense.h
deleted file mode 100644
index 586dd25..0000000
--- a/60xx/mlsdk/mllite/invensense.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/** Main header file for Invensense's basic library.
- */
-#include "accel.h"
-#include "compass.h"
-#include "dmpDefault.h"
-#include "dmpKey.h"
-#include "dmpmap.h"
-#include "ml.h"
-#include "mlBiasNoMotion.h"
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#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"
-#include "mldl_cfg.h"
-#include "mldmp.h"
-#include "mlinclude.h"
-#include "mlstates.h"
-#include "mlsupervisor.h"
-#include "pressure.h"
diff --git a/60xx/mlsdk/mllite/ml.c b/60xx/mlsdk/mllite/ml.c
deleted file mode 100644
index 3328edd..0000000
--- a/60xx/mlsdk/mllite/ml.c
+++ /dev/null
@@ -1,1885 +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.c 5642 2011-06-14 02:26:20Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML
- * @brief Motion Library APIs.
- * The Motion Library processes gyroscopes, accelerometers, and
- * compasses to provide a physical model of the movement for the
- * sensors.
- * The results of this processing may be used to control objects
- * within a user interface environment, detect gestures, track 3D
- * movement for gaming applications, and analyze the blur created
- * due to hand movement while taking a picture.
- *
- * @{
- * @file ml.c
- * @brief The Motion Library APIs.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-
-#include "ml.h"
-#include "mldl.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "compass.h"
-#include "dmpKey.h"
-#include "dmpDefault.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "mlMathFunc.h"
-#include "mlsupervisor.h"
-#include "mlmath.h"
-#include "mlcontrol.h"
-#include "mldl_cfg.h"
-#include "mpu.h"
-#include "accel.h"
-#include "mlos.h"
-#include "mlsl.h"
-#include "mlos.h"
-#include "mlBiasNoMotion.h"
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-ml"
-
-#define ML_MOT_TYPE_NONE 0
-#define ML_MOT_TYPE_NO_MOTION 1
-#define ML_MOT_TYPE_MOTION_DETECTED 2
-
-#define ML_MOT_STATE_MOVING 0
-#define ML_MOT_STATE_NO_MOTION 1
-#define ML_MOT_STATE_BIAS_IN_PROG 2
-
-#define _mlDebug(x) //{x}
-
-/* Global Variables */
-const unsigned char mlVer[] = { INV_VERSION };
-
-struct inv_params_obj inv_params_obj = {
- INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode
- INV_ORIENTATION_MASK_DEFAULT, // orientation_mask
- INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func
- INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func
- INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func
- INV_STATE_SERIAL_CLOSED // starting state
-};
-
-struct inv_obj_t inv_obj;
-void *g_mlsl_handle;
-
-typedef struct {
- // These describe callbacks happening everythime a DMP interrupt is processed
- int_fast8_t numInterruptProcesses;
- // Array of function pointers, function being void function taking void
- inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES];
-
-} tMLXCallbackInterrupt; // MLX_callback_t
-
-tMLXCallbackInterrupt mlxCallbackInterrupt;
-
-/* --------------- */
-/* - Functions. - */
-/* --------------- */
-
-inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient);
-inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient);
-unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
-
-/**
- * @brief Open serial connection with the MPU device.
- * This is the entry point of the MPL and must be
- * called prior to any other function call.
- *
- * @param port System handle for 'port' MPU device is found on.
- * The significance of this parameter varies by
- * platform. It is passed as 'port' to MLSLSerialOpen.
- *
- * @return INV_SUCCESS or error code.
- */
-inv_error_t inv_serial_start(char const *port)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
-
- if (inv_get_state() >= INV_STATE_SERIAL_OPENED)
- return INV_SUCCESS;
-
- result = inv_state_transition(INV_STATE_SERIAL_OPENED);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_serial_open(port, &g_mlsl_handle);
- if (INV_SUCCESS != result) {
- (void)inv_state_transition(INV_STATE_SERIAL_CLOSED);
- }
-
- return result;
-}
-
-/**
- * @brief Close the serial communication.
- * This function needs to be called explicitly to shut down
- * the communication with the device. Calling inv_dmp_close()
- * won't affect the established serial communication.
- * @return INV_SUCCESS; non-zero error code otherwise.
- */
-inv_error_t inv_serial_stop(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() == INV_STATE_SERIAL_CLOSED)
- return INV_SUCCESS;
-
- result = inv_state_transition(INV_STATE_SERIAL_CLOSED);
- if (INV_SUCCESS != result) {
- MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result);
- }
- result = inv_serial_close(g_mlsl_handle);
- if (INV_SUCCESS != result) {
- MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result);
- }
- return result;
-}
-
-/**
- * @brief Get the serial file handle to the device.
- * @return The serial file handle.
- */
-void *inv_get_serial_handle(void)
-{
- INVENSENSE_FUNC_START;
- return g_mlsl_handle;
-}
-
-/**
- * @brief apply the choosen orientation and full scale range
- * for gyroscopes, accelerometer, and compass.
- * @return INV_SUCCESS if successful, a non-zero code otherwise.
- */
-inv_error_t inv_apply_calibration(void)
-{
- INVENSENSE_FUNC_START;
- signed char gyroCal[9] = { 0 };
- signed char accelCal[9] = { 0 };
- signed char magCal[9] = { 0 };
- float gyroScale = 2000.f;
- float accelScale = 0.f;
- float magScale = 0.f;
-
- inv_error_t result;
- int ii;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- for (ii = 0; ii < 9; ii++) {
- gyroCal[ii] = mldl_cfg->pdata->orientation[ii];
- if (NULL != mldl_cfg->accel){
- accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii];
- }
- if (NULL != mldl_cfg->compass){
- magCal[ii] = mldl_cfg->pdata->compass.orientation[ii];
- }
- }
-
- switch (mldl_cfg->full_scale) {
- case MPU_FS_250DPS:
- gyroScale = 250.f;
- break;
- case MPU_FS_500DPS:
- gyroScale = 500.f;
- break;
- case MPU_FS_1000DPS:
- gyroScale = 1000.f;
- break;
- case MPU_FS_2000DPS:
- gyroScale = 2000.f;
- break;
- default:
- MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n",
- mldl_cfg->full_scale);
- return INV_ERROR_INVALID_PARAMETER;
- break;
- }
-
- if (NULL != mldl_cfg->accel){
- RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale);
- inv_obj.accel_sens = (long)(accelScale * 65536L);
- /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim;
-#else
- inv_obj.accel_sens /= 2;
-#endif
- }
- if (NULL != mldl_cfg->compass){
- RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale);
- inv_obj.compass_sens = (long)(magScale * 32768);
- }
-
- if (inv_get_state() == INV_STATE_DMP_OPENED) {
- result = inv_set_gyro_calibration(gyroScale, gyroCal);
- if (INV_SUCCESS != result) {
- MPL_LOGE("Unable to set Gyro Calibration\n");
- return result;
- }
- if (NULL != mldl_cfg->accel){
- result = inv_set_accel_calibration(accelScale, accelCal);
- if (INV_SUCCESS != result) {
- MPL_LOGE("Unable to set Accel Calibration\n");
- return result;
- }
- }
- if (NULL != mldl_cfg->compass){
- result = inv_set_compass_calibration(magScale, magCal);
- if (INV_SUCCESS != result) {
- MPL_LOGE("Unable to set Mag Calibration\n");
- return result;
- }
- }
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief Setup the DMP to handle the accelerometer endianess.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_apply_endian_accel(void)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[4] = { 0 };
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- int endian = mldl_cfg->accel->endian;
-
- if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) {
- endian = EXT_SLAVE_BIG_ENDIAN;
- }
- switch (endian) {
- case EXT_SLAVE_FS8_BIG_ENDIAN:
- case EXT_SLAVE_FS16_BIG_ENDIAN:
- case EXT_SLAVE_LITTLE_ENDIAN:
- regs[0] = 0;
- regs[1] = 64;
- regs[2] = 0;
- regs[3] = 0;
- break;
- case EXT_SLAVE_BIG_ENDIAN:
- default:
- regs[0] = 0;
- regs[1] = 0;
- regs[2] = 64;
- regs[3] = 0;
- }
-
- return inv_set_mpu_memory(KEY_D_1_236, 4, regs);
-}
-
-/**
- * @internal
- * @brief Initialize MLX data. This should be called to setup the mlx
- * output buffers before any motion processing is done.
- */
-void inv_init_ml(void)
-{
- INVENSENSE_FUNC_START;
- int ii;
- long tmp[COMPASS_NUM_AXES];
- // Set all values to zero by default
- memset(&inv_obj, 0, sizeof(struct inv_obj_t));
- memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt));
-
- inv_obj.compass_correction[0] = 1073741824L;
- inv_obj.compass_correction_relative[0] = 1073741824L;
- inv_obj.compass_disturb_correction[0] = 1073741824L;
- inv_obj.compass_correction_offset[0] = 1073741824L;
- inv_obj.relative_quat[0] = 1073741824L;
-
- //Not used with the ST accelerometer
- inv_obj.no_motion_threshold = 20; // noMotionThreshold
- //Not used with the ST accelerometer
- inv_obj.motion_duration = 1536; // motionDuration
-
- inv_obj.motion_state = INV_MOTION; // Motion state
-
- inv_obj.bias_update_time = 8000;
- inv_obj.bias_calc_time = 2000;
-
- inv_obj.internal_motion_state = ML_MOT_STATE_MOVING;
-
- inv_obj.start_time = inv_get_tick_count();
-
- inv_obj.compass_cal[0] = 322122560L;
- inv_obj.compass_cal[4] = 322122560L;
- inv_obj.compass_cal[8] = 322122560L;
- inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed.
-
- for (ii = 0; ii < COMPASS_NUM_AXES; ii++) {
- inv_obj.compass_scale[ii] = 65536L;
- inv_obj.compass_test_scale[ii] = 65536L;
- inv_obj.compass_bias_error[ii] = P_INIT;
- inv_obj.init_compass_bias[ii] = 0;
- inv_obj.compass_asa[ii] = (1L << 30);
- }
- if (inv_compass_read_scale(tmp) == INV_SUCCESS) {
- for (ii = 0; ii < COMPASS_NUM_AXES; ii++)
- inv_obj.compass_asa[ii] = tmp[ii];
- }
- inv_obj.got_no_motion_bias = 0;
- inv_obj.got_compass_bias = 0;
- inv_obj.compass_state = SF_UNCALIBRATED;
- inv_obj.acc_state = SF_STARTUP_SETTLE;
-
- inv_obj.got_init_compass_bias = 0;
- inv_obj.resetting_compass = 0;
-
- inv_obj.external_slave_callback = NULL;
- inv_obj.compass_accuracy = 0;
-
- inv_obj.factory_temp_comp = 0;
- inv_obj.got_coarse_heading = 0;
-
- inv_obj.compass_bias_ptr[0] = P_INIT;
- inv_obj.compass_bias_ptr[4] = P_INIT;
- inv_obj.compass_bias_ptr[8] = P_INIT;
-
- inv_obj.gyro_bias_err = 1310720;
-
- inv_obj.accel_lpf_gain = 1073744L;
- inv_obj.no_motion_accel_threshold = 7000000L;
-}
-
-/**
- * @internal
- * @brief This registers a function to be called for each time the DMP
- * generates an an interrupt.
- * It will be called after inv_register_fifo_rate_process() which gets called
- * every time the FIFO data is processed.
- * The FIFO does not have to be on for this callback.
- * @param func Function to be called when a DMP interrupt occurs.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func)
-{
- INVENSENSE_FUNC_START;
- // Make sure we have not filled up our number of allowable callbacks
- if (mlxCallbackInterrupt.numInterruptProcesses <=
- MAX_INTERRUPT_PROCESSES - 1) {
- int kk;
- // Make sure we haven't registered this function already
- for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
- if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- }
- // Add new callback
- mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt.
- numInterruptProcesses] = func;
- mlxCallbackInterrupt.numInterruptProcesses++;
- return INV_SUCCESS;
- }
- return INV_ERROR_MEMORY_EXAUSTED;
-}
-
-/**
- * @internal
- * @brief This unregisters a function to be called for each DMP interrupt.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func)
-{
- INVENSENSE_FUNC_START;
- int kk, jj;
- // Make sure we haven't registered this function already
- for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
- if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
- // FIXME, we may need a thread block here
- for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses;
- ++jj) {
- mlxCallbackInterrupt.processInterruptCb[jj - 1] =
- mlxCallbackInterrupt.processInterruptCb[jj];
- }
- mlxCallbackInterrupt.numInterruptProcesses--;
- return INV_SUCCESS;
- }
- }
- return INV_ERROR_INVALID_PARAMETER;
-}
-
-/**
- * @internal
- * @brief Run the recorded interrupt process callbacks in the event
- * of an interrupt.
- */
-void inv_run_dmp_interupt_cb(void)
-{
- int kk;
- for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
- if (mlxCallbackInterrupt.processInterruptCb[kk])
- mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj);
- }
-}
-
-/** @internal
-* Resets the Motion/No Motion state which should be called at startup and resume.
-*/
-inv_error_t inv_reset_motion(void)
-{
- unsigned char regs[8];
- inv_error_t result;
-
- inv_obj.motion_state = INV_MOTION;
- inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
- inv_obj.no_motion_accel_time = inv_get_tick_count();
-#if defined CONFIG_MPU_SENSORS_MPU3050
- regs[0] = DINAD8 + 2;
- regs[1] = DINA0C;
- regs[2] = DINAD8 + 1;
- result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#else
-#endif
- regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
- regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
- result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- memset(regs, 0, 8);
- result = inv_set_mpu_memory(KEY_D_1_96, 8, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result =
- inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- inv_set_motion_state(INV_MOTION);
- return result;
-}
-
-/**
- * @internal
- * @brief inv_start_bias_calc starts the bias calculation on the MPU.
- */
-void inv_start_bias_calc(void)
-{
- INVENSENSE_FUNC_START;
-
- inv_obj.suspend = 1;
-}
-
-/**
- * @internal
- * @brief inv_stop_bias_calc stops the bias calculation on the MPU.
- */
-void inv_stop_bias_calc(void)
-{
- INVENSENSE_FUNC_START;
-
- inv_obj.suspend = 0;
-}
-
-/**
- * @brief inv_update_data fetches data from the fifo and updates the
- * motion algorithms.
- *
- * @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.
- *
- * @note Motion algorithm data is constant between calls to inv_update_data
- *
- * @return
- * - INV_SUCCESS
- * - Non-zero error code
- */
-inv_error_t inv_update_data(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
- int_fast8_t got, ftry;
- uint_fast8_t mpu_interrupt;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (inv_get_state() != INV_STATE_DMP_STARTED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- // Set the maximum number of FIFO packets we want to process
- if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
- ftry = 100; // Large enough to process all packets
- } else {
- ftry = 1;
- }
-
- // Go and process at most ftry number of packets, probably less than ftry
- result = inv_read_and_process_fifo(ftry, &got);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- // Process all interrupts
- mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1);
- if (mpu_interrupt) {
- inv_clear_interrupt_trigger(INTSRC_AUX1);
- }
- // Check if interrupt was from MPU
- mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU);
- if (mpu_interrupt) {
- inv_clear_interrupt_trigger(INTSRC_MPU);
- }
- // Take care of the callbacks that want to be notified when there was an MPU interrupt
- if (mpu_interrupt) {
- inv_run_dmp_interupt_cb();
- }
-
- result = inv_get_fifo_status();
- return result;
-}
-
-/**
- * @brief inv_check_flag returns the value of a flag.
- * inv_check_flag can be used to check a number of flags,
- * allowing users to poll flags rather than register callback
- * functions. If a flag is set to True when inv_check_flag is called,
- * the flag is automatically reset.
- * The flags are:
- * - INV_RAW_DATA_READY
- * Indicates that new raw data is available.
- * - INV_PROCESSED_DATA_READY
- * Indicates that new processed data is available.
- * - INV_GOT_GESTURE
- * Indicates that a gesture has been detected by the gesture engine.
- * - INV_MOTION_STATE_CHANGE
- * Indicates that a change has been made from motion to no motion,
- * or vice versa.
- *
- * @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.
- *
- * @param flag The flag to check.
- *
- * @return TRUE or FALSE state of the flag
- */
-int inv_check_flag(int flag)
-{
- INVENSENSE_FUNC_START;
- int flagReturn = inv_obj.flags[flag];
-
- inv_obj.flags[flag] = 0;
- return flagReturn;
-}
-
-/**
- * @brief Enable generation of the DMP interrupt when Motion or no-motion
- * is detected
- * @param on
- * Boolean to turn the interrupt on or off.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_set_motion_interrupt(unsigned char on)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- unsigned char regs[2] = { 0 };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (on) {
- result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- inv_obj.interrupt_sources |= INV_INT_MOTION;
- } else {
- inv_obj.interrupt_sources &= ~INV_INT_MOTION;
- if (!inv_obj.interrupt_sources) {
- result = inv_get_dl_cfg_int(0);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- }
-
- if (on) {
- regs[0] = DINAFE;
- } else {
- regs[0] = DINAD8;
- }
- result = inv_set_mpu_memory(KEY_CFG_7, 1, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return result;
-}
-
-/**
- * Enable generation of the DMP interrupt when a FIFO packet is ready
- *
- * @param on Boolean to turn the interrupt on or off
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_set_fifo_interrupt(unsigned char on)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- unsigned char regs[2] = { 0 };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (on) {
- result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- inv_obj.interrupt_sources |= INV_INT_FIFO;
- } else {
- inv_obj.interrupt_sources &= ~INV_INT_FIFO;
- if (!inv_obj.interrupt_sources) {
- result = inv_get_dl_cfg_int(0);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- }
-
- if (on) {
- regs[0] = DINAFE;
- } else {
- regs[0] = DINAD8;
- }
- result = inv_set_mpu_memory(KEY_CFG_6, 1, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return result;
-}
-
-/**
- * @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
- * Note" document provided. Section 5, "Sensor Mounting Orientation"
- * offers a good coverage on the mounting matrices and explains how
- * to use them.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- * @pre inv_dmp_start() must <b>NOT</b> have been called.
- *
- * @see inv_set_gyro_calibration().
- * @see inv_set_compass_calibration().
- *
- * @param[in] range
- * The range of the accelerometers in g's. An accelerometer
- * that has a range of +2g's to -2g's should pass in 2.
- * @param[in] orientation
- * A 9 element matrix that represents how the accelerometers
- * are oriented with respect to the device they are mounted
- * in and the reference axis system.
- * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
- * This example corresponds to a 3 x 3 identity matrix.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_set_accel_calibration(float range, signed char *orientation)
-{
- INVENSENSE_FUNC_START;
- float cal[9];
- float scale = range / 32768.f;
- int kk;
- unsigned long sf;
- inv_error_t result;
- unsigned char regs[4] = { 0, 0, 0, 0 };
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (inv_get_state() != INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- /* Apply zero g-offset values */
- if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) {
- regs[0] = 0x80;
- regs[1] = 0x0;
- regs[2] = 0x80;
- regs[3] = 0x0;
- }
-
- if (inv_dmpkey_supported(KEY_D_1_152)) {
- result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- if (scale == 0) {
- inv_obj.accel_sens = 0;
- }
-
- if (mldl_cfg->accel->id) {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C };
-#else
- unsigned char tmp[3] = { DINA4C, DINACD, DINA6C };
-#endif
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- unsigned char regs[3];
- unsigned short orient;
-
- for (kk = 0; kk < 9; ++kk) {
- cal[kk] = scale * orientation[kk];
- inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
- }
-
- orient = inv_orientation_matrix_to_scalar(orientation);
- regs[0] = tmp[orient & 3];
- regs[1] = tmp[(orient >> 3) & 3];
- regs[2] = tmp[(orient >> 6) & 3];
- result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- regs[0] = DINA26;
- regs[1] = DINA46;
-#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2
- regs[2] = DINA66;
-#else
- if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision)
- == MPU_PRODUCT_KEY_B1_E1_5)
- regs[2] = DINA76;
- else
- regs[2] = DINA66;
-#endif
- if (orient & 4)
- regs[0] |= 1;
- if (orient & 0x20)
- regs[1] |= 1;
- if (orient & 0x100)
- regs[2] |= 1;
-
- result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) {
- result = inv_freescale_sensor_fusion_16bit(orient);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) {
- result = inv_freescale_sensor_fusion_8bit(orient);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- }
-
- if (inv_obj.accel_sens != 0) {
- sf = (1073741824L / inv_obj.accel_sens);
- } else {
- sf = 0;
- }
- regs[0] = (unsigned char)((sf >> 8) & 0xff);
- regs[1] = (unsigned char)(sf & 0xff);
- result = inv_set_mpu_memory(KEY_D_0_108, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-/**
- * @brief Sets up the Gyro calibration and scale factor.
- *
- * Please refer to the provided "9-Axis Sensor Fusion Application
- * Note" document provided. Section 5, "Sensor Mounting Orientation"
- * offers a good coverage on the mounting matrices and explains
- * how to use them.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- * @pre inv_dmp_start() must have <b>NOT</b> been called.
- *
- * @see inv_set_accel_calibration().
- * @see inv_set_compass_calibration().
- *
- * @param[in] range
- * The range of the gyros in degrees per second. A gyro
- * that has a range of +2000 dps to -2000 dps should pass in
- * 2000.
- * @param[in] orientation
- * A 9 element matrix that represents how the gyro are oriented
- * with respect to the device they are mounted in. A typical
- * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This
- * example corresponds to a 3 x 3 identity matrix.
- *
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_gyro_calibration(float range, signed char *orientation)
-{
- INVENSENSE_FUNC_START;
-
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- int kk;
- float scale;
- inv_error_t result;
-
- unsigned char regs[12] = { 0 };
- unsigned char maxVal = 0;
- unsigned char tmpPtr = 0;
- unsigned char tmpSign = 0;
- unsigned char i = 0;
- int sf = 0;
-
- if (inv_get_state() != INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (mldl_cfg->gyro_sens_trim != 0) {
- /* adjust the declared range to consider the gyro_sens_trim
- of this part when different from the default (first dividend) */
- range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim;
- }
-
- scale = range / 32768.f; // inverse of sensitivity for the given full scale range
- inv_obj.gyro_sens = (long)(range * 32768L);
-
- for (kk = 0; kk < 9; ++kk) {
- inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30));
- inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30));
- }
-
- {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- unsigned char tmpD = DINA4C;
- unsigned char tmpE = DINACD;
- unsigned char tmpF = DINA6C;
-#else
- unsigned char tmpD = DINAC9;
- unsigned char tmpE = DINA2C;
- unsigned char tmpF = DINACB;
-#endif
- regs[3] = DINA36;
- regs[4] = DINA56;
- regs[5] = DINA76;
-
- for (i = 0; i < 3; i++) {
- maxVal = 0;
- tmpSign = 0;
- if (inv_obj.gyro_orient[0 + 3 * i] < 0)
- tmpSign = 1;
- if (ABS(inv_obj.gyro_orient[1 + 3 * i]) >
- ABS(inv_obj.gyro_orient[0 + 3 * i])) {
- maxVal = 1;
- if (inv_obj.gyro_orient[1 + 3 * i] < 0)
- tmpSign = 1;
- }
- if (ABS(inv_obj.gyro_orient[2 + 3 * i]) >
- ABS(inv_obj.gyro_orient[1 + 3 * i])) {
- tmpSign = 0;
- maxVal = 2;
- if (inv_obj.gyro_orient[2 + 3 * i] < 0)
- tmpSign = 1;
- }
- if (maxVal == 0) {
- regs[tmpPtr++] = tmpD;
- if (tmpSign)
- regs[tmpPtr + 2] |= 0x01;
- } else if (maxVal == 1) {
- regs[tmpPtr++] = tmpE;
- if (tmpSign)
- regs[tmpPtr + 2] |= 0x01;
- } else {
- regs[tmpPtr++] = tmpF;
- if (tmpSign)
- regs[tmpPtr + 2] |= 0x01;
- }
- }
- result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384
- inv_obj.gyro_sf =
- (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL);
- result =
- inv_set_mpu_memory(KEY_D_0_104, 4,
- inv_int32_to_big8(inv_obj.gyro_sf, regs));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- if (inv_obj.gyro_sens != 0) {
- sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens);
- } else {
- sf = 0;
- }
-
- result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief Sets up the Compass calibration and scale factor.
- *
- * Please refer to the provided "9-Axis Sensor Fusion Application
- * Note" document provided. Section 5, "Sensor Mounting Orientation"
- * offers a good coverage on the mounting matrices and explains
- * how to use them.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- * @pre inv_dmp_start() must have <b>NOT</b> been called.
- *
- * @see inv_set_gyro_calibration().
- * @see inv_set_accel_calibration().
- *
- * @param[in] range
- * The range of the compass.
- * @param[in] orientation
- * A 9 element matrix that represents how the compass is
- * oriented with respect to the device they are mounted in.
- * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
- * This example corresponds to a 3 x 3 identity matrix.
- * The matrix describes how to go from the chip mounting to
- * the body of the device.
- *
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_compass_calibration(float range, signed char *orientation)
-{
- INVENSENSE_FUNC_START;
- 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];
- inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
- }
-
- inv_obj.compass_sens = (long)(scale * 1073741824L);
-
- if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) {
- unsigned char reg0[4] = { 0, 0, 0, 0 };
- unsigned char regp[4] = { 64, 0, 0, 0 };
- unsigned char regn[4] = { 64 + 128, 0, 0, 0 };
- unsigned char *reg;
- int_fast8_t kk;
- unsigned short keyList[9] =
- { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02,
- KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12,
- KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22
- };
-
- for (kk = 0; kk < 9; ++kk) {
-
- if (orientation[kk] == 1)
- reg = regp;
- else if (orientation[kk] == -1)
- reg = regn;
- else
- reg = reg0;
- inv_set_mpu_memory(keyList[kk], 4, reg);
- }
- }
-
- return INV_SUCCESS;
-}
-
-/**
-* @internal
-* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup.
-*/
-inv_error_t inv_set_dead_zone(void)
-{
- unsigned char reg;
- inv_error_t result;
- extern struct control_params cntrl_params;
-
- if (cntrl_params.functions & INV_DEAD_ZONE) {
- reg = 0x08;
- } else {
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- reg = 0;
-#else
- if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
- reg = 0x2;
- } else {
- reg = 0;
- }
-#endif
- }
-
- result = inv_set_mpu_memory(KEY_D_0_163, 1, ®);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return result;
-}
-
-/**
- * @brief inv_set_bias_update is used to register which algorithms will be
- * used to automatically reset the gyroscope bias.
- * The engine INV_BIAS_UPDATE must be enabled for these algorithms to
- * run.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * and inv_dmp_start()
- * must <b>NOT</b> have been called.
- *
- * @param function A function or bitwise OR of functions that determine
- * how the gyroscope bias will be automatically updated.
- * Functions include:
- * - INV_NONE or 0,
- * - INV_BIAS_FROM_NO_MOTION,
- * - INV_BIAS_FROM_GRAVITY,
- * - INV_BIAS_FROM_TEMPERATURE,
- \ifnot UMPL
- * - INV_BIAS_FROM_LPF,
- * - INV_MAG_BIAS_FROM_MOTION,
- * - INV_MAG_BIAS_FROM_GYRO,
- * - INV_ALL.
- * \endif
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_bias_update(unsigned short function)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[4];
- long tmp[3] = { 0, 0, 0 };
- inv_error_t result = INV_SUCCESS;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (inv_get_state() != INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- /* do not allow progressive no motion bias tracker to run -
- it's not fully debugged */
- function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround
- MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n");
-
- // You must use EnableFastNoMotion() to get this feature
- function &= ~INV_BIAS_FROM_FAST_NO_MOTION;
-
- // You must use DisableFastNoMotion() to turn this feature off
- if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION)
- function |= INV_BIAS_FROM_FAST_NO_MOTION;
-
- /*--- remove magnetic components from bias tracking
- if there is no compass ---*/
- if (!inv_compass_present()) {
- function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION);
- } else {
- function &= ~(INV_BIAS_FROM_LPF);
- }
-
- // Enable function sets bias from no motion
- inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION);
-
- if (function & INV_BIAS_FROM_NO_MOTION) {
- inv_enable_bias_no_motion();
- } else {
- inv_disable_bias_no_motion();
- }
-
- if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
- regs[0] = DINA80 + 2;
- regs[1] = DINA2D;
- regs[2] = DINA55;
- regs[3] = DINA7D;
- } else {
- regs[0] = DINA80 + 7;
- regs[1] = DINA2D;
- regs[2] = DINA35;
- regs[3] = DINA3D;
- }
- result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_dead_zone();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) &&
- !inv_compass_present()) {
- result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else {
- result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- inv_obj.factory_temp_comp = 0; // FIXME, workaround
- if ((mldl_cfg->offset_tc[0] != 0) ||
- (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) {
- inv_obj.factory_temp_comp = 1;
- }
-
- if (inv_obj.factory_temp_comp == 0) {
- if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) {
- result = inv_set_gyro_temp_slope(inv_obj.temp_slope);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else {
- result = inv_set_gyro_temp_slope(tmp);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- } else {
- inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE;
- MPL_LOGV("factory temperature compensation coefficients available - "
- "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n");
- }
-
- /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on
- compass and accel data, is to have accelerometer data delivered in the
- FIFO ----*/
- if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY)
- && inv_compass_present())
- || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO)
- || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) {
- inv_send_accel(INV_ALL, INV_32_BIT);
- inv_send_gyro(INV_ALL, INV_32_BIT);
- }
-
- return result;
-}
-
-/**
- * @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
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param thresh A threshold scaled in degrees per second.
- *
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_no_motion_thresh(float thresh)
-{
- inv_error_t result = INV_SUCCESS;
- unsigned char regs[4] = { 0 };
- long tmp;
- INVENSENSE_FUNC_START;
-
- tmp = (long)(thresh * thresh * 2.045f);
- if (tmp < 0) {
- return INV_ERROR;
- } else if (tmp > 8180000L) {
- return INV_ERROR;
- }
- inv_obj.no_motion_threshold = tmp;
-
- regs[0] = (unsigned char)(tmp >> 24);
- regs[1] = (unsigned char)((tmp >> 16) & 0xff);
- regs[2] = (unsigned char)((tmp >> 8) & 0xff);
- regs[3] = (unsigned char)(tmp & 0xff);
-
- result = inv_set_mpu_memory(KEY_D_1_108, 4, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_reset_motion();
- return result;
-}
-
-/**
- * @brief inv_set_no_motion_threshAccel is used to set the threshold for
- * detecting INV_NO_MOTION with accelerometers when Gyros have
- * been turned off
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param thresh A threshold in g's scaled by 2^32
- *
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_no_motion_threshAccel(long thresh)
-{
- INVENSENSE_FUNC_START;
-
- inv_obj.no_motion_accel_threshold = thresh;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_set_no_motion_time is used to set the time required for
- * detecting INV_NO_MOTION
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param time A time in seconds.
- *
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_no_motion_time(float time)
-{
- inv_error_t result = INV_SUCCESS;
- unsigned char regs[2] = { 0 };
- long tmp;
-
- INVENSENSE_FUNC_START;
-
- tmp = (long)(time * 200);
- if (tmp < 0) {
- return INV_ERROR;
- } else if (tmp > 65535L) {
- return INV_ERROR;
- }
- inv_obj.motion_duration = (unsigned short)tmp;
-
- regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
- regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
- result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_reset_motion();
- return result;
-}
-
-/**
- * @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
- * dynamically based on calls to inv_set_mpu_sensors().
- *
- * @return TRUE if the gyro is enabled FALSE otherwise.
- */
-int inv_get_gyro_present(void)
-{
- return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO |
- INV_Z_GYRO);
-}
-
-static unsigned short inv_row_2_scale(const signed char *row)
-{
- unsigned short b;
-
- if (row[0] > 0)
- b = 0;
- else if (row[0] < 0)
- b = 4;
- else if (row[1] > 0)
- b = 1;
- else if (row[1] < 0)
- b = 5;
- else if (row[2] > 0)
- b = 2;
- else if (row[2] < 0)
- b = 6;
- else
- b = 7; // error
- return b;
-}
-
-unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
-{
- unsigned short scalar;
- /*
- XYZ 010_001_000 Identity Matrix
- XZY 001_010_000
- YXZ 010_000_001
- YZX 000_010_001
- ZXY 001_000_010
- ZYX 000_001_010
- */
-
- scalar = inv_row_2_scale(mtx);
- scalar |= inv_row_2_scale(mtx + 3) << 3;
- scalar |= inv_row_2_scale(mtx + 6) << 6;
-
- return scalar;
-}
-
-/* Setups up the Freescale 16-bit accel for Sensor Fusion
-* @param[in] orient A scalar representation of the orientation
-* that describes how to go from the Chip Orientation
-* to the Board Orientation often times called the Body Orientation in Invensense Documentation.
-* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
-*/
-inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient)
-{
- unsigned char rr[3];
- inv_error_t result;
-
- orient = orient & 0xdb;
- switch (orient) {
- default:
- // Typically 0x88
- rr[0] = DINACC;
- rr[1] = DINACF;
- rr[2] = DINA0E;
- break;
- case 0x50:
- rr[0] = DINACE;
- rr[1] = DINA0E;
- rr[2] = DINACD;
- break;
- case 0x81:
- rr[0] = DINACE;
- rr[1] = DINACB;
- rr[2] = DINA0E;
- break;
- case 0x11:
- rr[0] = DINACC;
- rr[1] = DINA0E;
- rr[2] = DINACB;
- break;
- case 0x42:
- rr[0] = DINA0A;
- rr[1] = DINACF;
- rr[2] = DINACB;
- break;
- case 0x0a:
- rr[0] = DINA0A;
- rr[1] = DINACB;
- rr[2] = DINACD;
- break;
- }
- result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr);
- return result;
-}
-
-/* Setups up the Freescale 8-bit accel for Sensor Fusion
-* @param[in] orient A scalar representation of the orientation
-* that describes how to go from the Chip Orientation
-* to the Board Orientation often times called the Body Orientation in Invensense Documentation.
-* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
-*/
-inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient)
-{
- unsigned char regs[27];
- inv_error_t result;
- uint_fast8_t kk;
-
- orient = orient & 0xdb;
- kk = 0;
-
- regs[kk++] = DINAC3;
- regs[kk++] = DINA90 + 14;
- regs[kk++] = DINAA0 + 9;
- regs[kk++] = DINA3E;
- regs[kk++] = DINA5E;
- regs[kk++] = DINA7E;
-
- regs[kk++] = DINAC2;
- regs[kk++] = DINAA0 + 9;
- regs[kk++] = DINA90 + 9;
- regs[kk++] = DINAF8 + 2;
-
- switch (orient) {
- default:
- // Typically 0x88
- regs[kk++] = DINACB;
-
- regs[kk++] = DINA54;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
-
- regs[kk++] = DINACD;
- break;
- case 0x50:
- regs[kk++] = DINACB;
-
- regs[kk++] = DINACF;
-
- regs[kk++] = DINA7C;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- break;
- case 0x81:
- regs[kk++] = DINA2C;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
-
- regs[kk++] = DINACD;
-
- regs[kk++] = DINACB;
- break;
- case 0x11:
- regs[kk++] = DINA2C;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINA28;
- regs[kk++] = DINACB;
- regs[kk++] = DINACF;
- break;
- case 0x42:
- regs[kk++] = DINACF;
- regs[kk++] = DINACD;
-
- regs[kk++] = DINA7C;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- regs[kk++] = DINA78;
- break;
- case 0x0a:
- regs[kk++] = DINACD;
-
- regs[kk++] = DINA54;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
- regs[kk++] = DINA50;
-
- regs[kk++] = DINACF;
- break;
- }
-
- regs[kk++] = DINA90 + 7;
- regs[kk++] = DINAF8 + 3;
- regs[kk++] = DINAA0 + 9;
- regs[kk++] = DINA0E;
- regs[kk++] = DINA0E;
- regs[kk++] = DINA0E;
-
- regs[kk++] = DINAF8 + 1; // filler
-
- result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-/**
- * Controlls each sensor and each axis when the motion processing unit is
- * running. When it is not running, simply records the state for later.
- *
- * NOTE: In this version only full sensors controll is allowed. Independent
- * axis control will return an error.
- *
- * @param sensors Bit field of each axis desired to be turned on or off
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_set_mpu_sensors(unsigned long sensors)
-{
- INVENSENSE_FUNC_START;
- unsigned char state = inv_get_state();
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- inv_error_t result = INV_SUCCESS;
- unsigned short fifoRate;
-
- if (state < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) &&
- ((sensors & INV_THREE_AXIS_ACCEL) != 0)) {
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- }
- if (((sensors & INV_THREE_AXIS_ACCEL) != 0) &&
- (mldl_cfg->pdata->accel.get_slave_descr == 0)) {
- return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
- }
-
- if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) &&
- ((sensors & INV_THREE_AXIS_COMPASS) != 0)) {
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- }
- if (((sensors & INV_THREE_AXIS_COMPASS) != 0) &&
- (mldl_cfg->pdata->compass.get_slave_descr == 0)) {
- return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
- }
-
- if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) &&
- ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) {
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- }
- if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) &&
- (mldl_cfg->pdata->pressure.get_slave_descr == 0)) {
- return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
- }
-
- /* DMP was off, and is turning on */
- if (sensors & INV_DMP_PROCESSOR &&
- !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
- struct ext_slave_config config;
- long odr;
- config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
- config.len = sizeof(long);
- config.apply = (state == INV_STATE_DMP_STARTED);
- config.data = &odr;
-
- odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000);
- result = inv_mpu_config_accel(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
- odr = MPU_SLAVE_IRQ_TYPE_NONE;
- result = inv_mpu_config_accel(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- inv_init_fifo_hardare();
- }
-
- if (inv_obj.mode_change_func) {
- result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- /* Get the fifo rate before changing sensors so if we need to match it */
- fifoRate = inv_get_fifo_rate();
- mldl_cfg->requested_sensors = sensors;
-
- /* inv_dmp_start will turn the sensors on */
- if (state == INV_STATE_DMP_STARTED) {
- result = inv_dl_start(sensors);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_reset_motion();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_dl_stop(~sensors);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- inv_set_fifo_rate(fifoRate);
-
- if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) {
- struct ext_slave_config config;
- long data;
-
- config.len = sizeof(long);
- config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
- config.apply = (state == INV_STATE_DMP_STARTED);
- config.data = &data;
- data = MPU_SLAVE_IRQ_TYPE_DATA_READY;
- result = inv_mpu_config_accel(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- return result;
-}
-
-void inv_set_mode_change(inv_error_t(*mode_change_func)
- (unsigned long, unsigned long))
-{
- inv_obj.mode_change_func = mode_change_func;
-}
-
-/**
-* Mantis setup
-*/
-#ifdef CONFIG_MPU_SENSORS_MPU6050B1
-inv_error_t inv_set_mpu_6050_config()
-{
- long temp;
- inv_error_t result;
- unsigned char big8[4];
- unsigned char atc[4];
- long s[3], s2[3];
- int kk;
- struct mldl_cfg *mldlCfg = inv_get_dl_config();
-
- result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
- 0x0d, 4, atc);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- temp = atc[3] & 0x3f;
- if (temp >= 32)
- temp = temp - 64;
- temp = (temp << 21) | 0x100000;
- temp += (1L << 29);
- temp = -temp;
- result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- for (kk = 0; kk < 3; ++kk) {
- s[kk] = atc[kk] & 0x3f;
- if (s[kk] > 32)
- s[kk] = s[kk] - 64;
- s[kk] *= 2516582L;
- }
-
- for (kk = 0; kk < 3; ++kk) {
- s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] +
- mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] +
- mldlCfg->pdata->orientation[kk * 3 + 2] * s[2];
- }
- result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-#endif
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/ml.h b/60xx/mlsdk/mllite/ml.h
deleted file mode 100644
index 838cd49..0000000
--- a/60xx/mlsdk/mllite/ml.h
+++ /dev/null
@@ -1,596 +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.h 5653 2011-06-16 21:06:55Z nroyer $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML
- * @brief The Motion Library processes gyroscopes and accelerometers to
- * provide a physical model of the movement of the sensors.
- * The results of this processing may be used to control objects
- * within a user interface environment, detect gestures, track 3D
- * movement for gaming applications, and analyze the blur created
- * due to hand movement while taking a picture.
- *
- * @{
- * @file ml.h
- * @brief Header file for the Motion Library.
-**/
-
-#ifndef INV_H
-#define INV_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "mldmp.h"
-#include "mlsl.h"
-#include "mpu.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "ml_legacy.h"
-#endif
-
-/* ------------ */
-/* - Defines. - */
-/* ------------ */
-
-/* - Module defines. - */
-
-/*************************************************************************/
-/* Motion Library Vesion */
-/*************************************************************************/
-
-#define INV_VERSION_MAJOR 4
-#define INV_VERSION_MINOR 0
-#define INV_VERSION_SUB_MINOR 0
-
-#define INV_VERSION_MAJOR_STR "4"
-#define INV_VERSION_MINOR_STR "0"
-#define INV_VERSION_SUB_MINOR_STR "0"
-
-#define INV_VERSION_NONE ""
-#define INV_VERSION_PROTOTYPE "ProtoA "
-#define INV_VERSION_ENGINEERING "EngA "
-#define INV_VERSION_PRE_ALPHA "Pre-Alpha "
-#define INV_VERSION_ALPHA "Alpha "
-#define INV_VERSION_BETA "Beta "
-#define INV_VERSION_PRODUCTION "Production "
-
-#ifndef INV_VERSION_TYPE
-#define INV_VERSION_TYPE INV_VERSION_NONE
-#endif
-
-#define INV_VERSION "InvenSense MPL" " " \
- "v" INV_VERSION_MAJOR_STR "." INV_VERSION_MINOR_STR "." INV_VERSION_SUB_MINOR_STR " " \
- INV_VERSION_TYPE \
- __DATE__ " " __TIME__
-
-/*************************************************************************/
-/* Motion processing engines */
-/*************************************************************************/
-#define INV_MOTION_DETECT (0x0004)
-#define INV_BIAS_UPDATE (0x0008)
-#define INV_GESTURE (0x0020)
-#define INV_CONTROL (0x0040)
-#define INV_ORIENTATION (0x0100)
-#define INV_PEDOMETER (0x0200)
-#define INV_BASIC (INV_MOTION_DETECT | INV_BIAS_UPDATE)
-
-/*************************************************************************/
-/* Data Source - Obsolete */
-/*************************************************************************/
-#define INV_DATA_FIFO (0x1)
-#define INV_DATA_POLL (0x2)
-
-/*************************************************************************/
-/* Interrupt Source */
-/*************************************************************************/
-#define INV_INT_MOTION (0x01)
-#define INV_INT_FIFO (0x02)
-#define INV_INT_TAP (0x04)
-#define INV_INT_ORIENTATION (0x08)
-#define INV_INT_SHAKE_PITCH (0x10)
-#define INV_INT_SHAKE_ROLL (0x20)
-#define INV_INT_SHAKE_YAW (0x40)
-
-/*************************************************************************/
-/* Bias update functions */
-/*************************************************************************/
-#define INV_BIAS_FROM_NO_MOTION 0x0001
-#define INV_BIAS_FROM_GRAVITY 0x0002
-#define INV_BIAS_FROM_TEMPERATURE 0x0004
-#define INV_BIAS_FROM_LPF 0x0008
-#define INV_MAG_BIAS_FROM_MOTION 0x0010
-#define INV_MAG_BIAS_FROM_GYRO 0x0020
-#define INV_LEARN_BIAS_FROM_TEMPERATURE 0x0040
-#define INV_AUTO_RESET_MAG_BIAS 0x0080
-#define INV_REJECT_MAG_DISTURBANCE 0x0100
-#define INV_PROGRESSIVE_NO_MOTION 0x0200
-#define INV_BIAS_FROM_FAST_NO_MOTION 0x0400
-
-/*************************************************************************/
-/* Euler angles and axis names */
-/*************************************************************************/
-#define INV_X_AXIS (0x01)
-#define INV_Y_AXIS (0x02)
-#define INV_Z_AXIS (0x04)
-
-#define INV_PITCH (INV_X_AXIS)
-#define INV_ROLL (INV_Y_AXIS)
-#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
-#define INV_RESET 0x0001
-
-/*************************************************************************/
-/* Motion states */
-/*************************************************************************/
-#define INV_MOTION 0x0001
-#define INV_NO_MOTION 0x0002
-
-/*************************************************************************/
-/* Orientation and Gesture states */
-/*************************************************************************/
-#define INV_STATE_IDLE (0)
-#define INV_STATE_RUNNING (1)
-
-/*************************************************************************/
-/* Gyroscope Temperature Compensation bins */
-/*************************************************************************/
-#define BINS (25)
-#define PTS_PER_BIN (5)
-#define MIN_TEMP (-40)
-#define MAX_TEMP (+85)
-#define TEMP_PER_BIN ((MAX_TEMP - MIN_TEMP) / BINS)
-
-/*************************************************************************/
-/* Flags */
-/*************************************************************************/
-#define INV_RAW_DATA_READY 0x0001
-#define INV_PROCESSED_DATA_READY 0x0002
-
-#define INV_GOT_GESTURE 0x0004
-
-#define INV_MOTION_STATE_CHANGE 0x0006
-#define INV_COMPASS_OFFSET_VALID 0x0007
-
-/*************************************************************************/
-/* General */
-/*************************************************************************/
-#define INV_NONE (0x0000)
-#define INV_INVALID_FIFO_RATE (0xFFFF)
-
-/*************************************************************************/
-/* ML Params Structure Default Values */
-/*************************************************************************/
-#define INV_BIAS_UPDATE_FUNC_DEFAULT (INV_BIAS_FROM_NO_MOTION | INV_BIAS_FROM_GRAVITY)
-#define INV_ORIENTATION_MASK_DEFAULT 0x3f
-#define INV_PROCESSED_DATA_CALLBACK_DEFAULT 0
-#define INV_ORIENTATION_CALLBACK_DEFAULT 0
-#define INV_MOTION_CALLBACK_DEFAULT 0
-
-/* ------------ */
-/* - Defines. - */
-/* ------------ */
-/* Priority for various features */
-#define INV_PRIORITY_BUS_ACCEL 100
-#define INV_PRIORITY_EXTERNAL_SLAVE_MAG 110
-#define INV_PRIORITY_FAST_NO_MOTION 120
-#define INV_PRIORITY_BIAS_NO_MOTION 125
-#define INV_PRIORITY_SET_GYRO_BIASES 150
-#define INV_PRIORITY_TEMP_COMP 175
-#define INV_PRIORITY_CONTROL 200
-#define INV_PRIORITY_EIS 300
-#define INV_PRIORITY_ORIENTATION 400
-#define INV_PRIORITY_PEDOMETER_FULLPOWER 500
-#define INV_PRIORITY_NAVIGATION_PEDOMETER 600
-#define INV_PRIORITY_GESTURE 700
-#define INV_PRIORITY_GLYPH 800
-
-#define MAX_INTERRUPT_PROCESSES 5
-/* Number of quantized accel samples */
-#define INV_MAX_NUM_ACCEL_SAMPLES (8)
-
-#define PRECISION 10000.f
-#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) { \
- range.mantissa = (long)x; \
- range.fraction = (long)((float)(x-(long)x)*PRECISION); \
-}
-#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) { \
- x = (float)(range.mantissa); \
- x += ((float)range.fraction/PRECISION); \
-}
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
-struct inv_obj_t {
- //Calibration parameters
- /* Raw sensor orientation */
- long gyro_bias[3];
- long accel_bias[3];
- long compass_bias[3];
-
- /* Cached values after orietnation is applied */
- long scaled_gyro_bias[3];
- long scaled_accel_bias[3];
- long scaled_compass_bias[3];
-
- long compass_scale[3];
- long compass_test_bias[3];
- long compass_test_scale[3];
- long compass_asa[3];
- long compass_offsets[3];
-
- long compass_bias_error[3];
-
- long got_no_motion_bias;
- long got_compass_bias;
- long compass_state;
- long large_field;
- long acc_state;
-
- long factory_temp_comp;
- long got_coarse_heading;
- long gyro_temp_bias[3];
- long prog_no_motion_bias[3];
-
- long accel_cal[9];
- // Deprecated, used gyro_orient
- long gyro_cal[GYRO_NUM_AXES * GYRO_NUM_AXES];
- long gyro_orient[GYRO_NUM_AXES * GYRO_NUM_AXES];
- long accel_sens;
- long compass_cal[9];
- long gyro_sens;
- long gyro_sf;
- long temp_slope[GYRO_NUM_AXES];
- long compass_sens;
- long temp_offset[GYRO_NUM_AXES];
-
- int cal_loaded_flag;
-
- /* temperature compensation */
- float x_gyro_coef[3];
- float y_gyro_coef[3];
- float z_gyro_coef[3];
- float x_gyro_temp_data[BINS][PTS_PER_BIN];
- float y_gyro_temp_data[BINS][PTS_PER_BIN];
- float z_gyro_temp_data[BINS][PTS_PER_BIN];
- float temp_data[BINS][PTS_PER_BIN];
- int temp_ptrs[BINS];
- long temp_valid_data[BINS];
-
- long compass_correction[4];
- long compass_correction_relative[4];
- long compass_disturb_correction[4];
- long compass_correction_offset[4];
- long relative_quat[4];
- long local_field[3];
- long new_local_field;
- long sync_grav_body[3];
- int gyro_bias_err;
-
- double compass_bias_ptr[9];
- double compass_bias_v[3];
- double compass_prev_m[36];
- double compass_prev_xty[6];
-
- int compass_peaks[18];
- int all_sensors_no_motion;
-
- long init_compass_bias[3];
-
- int got_init_compass_bias;
- int resetting_compass;
-
- long ang_v_body[GYRO_NUM_AXES];
- unsigned char compass_raw_data[24]; /* Sensor data plus status etc */
- long compass_sensor_data[3]; /* Raw sensor data only */
- long compass_calibrated_data[3];
- long compass_test_calibrated_data[3];
- long pressure;
- inv_error_t (*external_slave_callback)(struct inv_obj_t *);
- int compass_accuracy;
- int compass_overunder;
-
- unsigned short flags[8];
- unsigned short suspend;
-
- long no_motion_threshold;
- unsigned long motion_duration;
-
- unsigned short motion_state;
-
- unsigned short data_mode;
- unsigned short interrupt_sources;
-
- unsigned short bias_update_time;
- short last_motion;
- unsigned short bias_calc_time;
-
- unsigned char internal_motion_state;
- long start_time;
-
- long accel_lpf_gain;
- long accel_lpf[3];
- unsigned long poll_no_motion;
- long no_motion_accel_threshold;
- unsigned long no_motion_accel_time;
- inv_error_t(*mode_change_func) (unsigned long, unsigned long);
- };
-
- typedef inv_error_t(*inv_obj_func) (struct inv_obj_t *);
-
- extern struct inv_obj_t inv_obj;
-
- /* --------------------- */
- /* - Params Structure. - */
- /* --------------------- */
-
- struct inv_params_obj {
-
- unsigned short bias_mode; // A function or bitwise OR of functions that determine how the gyroscope bias will be automatically updated.
- // Functions include INV_BIAS_FROM_NO_MOTION, INV_BIAS_FROM_GRAVITY, and INV_BIAS_FROM_TEMPERATURE.
- // The engine INV_BIAS_UPDATE must be enabled for these algorithms to run.
-
- unsigned short orientation_mask; // Allows a user to register which orientations will trigger the user defined callback function.
- // The orientations are INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, and INV_Z_DOWN.
- // INV_ORIENTATION_ALL is equivalent to INV_X_UP | INV_X_DOWN | INV_Y_UP | INV_Y_DOWN | INV_Z_UP | INV_Z_DOWN.
-
- void (*fifo_processed_func) (void); // Callback function that triggers when all the processing has been finished by the motion processing engines.
-
- void (*orientation_cb_func) (unsigned short orient); // Callback function that will run when a change of orientation is detected.
- // The new orientation. May be one of INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, or INV_Z_DOWN.
-
- void (*motion_cb_func) (unsigned short motion_state); // Callback function that will run when a change of motion state is detected.
- // The new motion state. May be one of INV_MOTION, or INV_NO_MOTION.
-
- unsigned char state;
-
- };
-
- extern struct inv_params_obj inv_params_obj;
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-
- inv_error_t inv_serial_start(char const *port);
- inv_error_t inv_serial_stop(void);
- inv_error_t inv_set_mpu_sensors(unsigned long sensors);
- void *inv_get_serial_handle(void);
-
- /*API for handling the buffer */
- inv_error_t inv_update_data(void);
-
- /*API for handling polling */
- int inv_check_flag(int flag);
-
- /*API for setting bias update function */
- inv_error_t inv_set_bias_update(unsigned short biasFunction);
-
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- inv_error_t inv_turn_on_bias_from_no_motion(void);
- inv_error_t inv_turn_off_bias_from_no_motion(void);
- inv_error_t inv_set_mpu_6050_config(void);
-#endif
-
- /* 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 */
-
-
- inv_error_t inv_get_gyro(long *data);
- inv_error_t inv_get_accel(long *data);
- inv_error_t inv_get_temperature(long *data);
- inv_error_t inv_get_temperature_raw(short *data);
- inv_error_t inv_get_rot_mat(long *data);
- inv_error_t inv_get_quaternion(long *data);
- inv_error_t inv_get_linear_accel(long *data);
- inv_error_t inv_get_linear_accel_in_world(long *data);
- inv_error_t inv_get_gravity(long *data);
- inv_error_t inv_get_angular_velocity(long *data);
- inv_error_t inv_get_euler_angles(long *data);
- inv_error_t inv_get_euler_angles_x(long *data);
- inv_error_t inv_get_euler_angles_y(long *data);
- inv_error_t inv_get_euler_angles_z(long *data);
- inv_error_t inv_get_gyro_temp_slope(long *data);
- inv_error_t inv_get_gyro_bias(long *data);
- inv_error_t inv_get_accel_bias(long *data);
- inv_error_t inv_get_mag_bias(long *data);
- inv_error_t inv_get_gyro_and_accel_sensor(long *data);
- inv_error_t inv_get_mag_raw_data(long *data);
- inv_error_t inv_get_magnetometer(long *data);
- inv_error_t inv_get_pressure(long *data);
- inv_error_t inv_get_heading(long *data);
- inv_error_t inv_get_gyro_cal_matrix(long *data);
- inv_error_t inv_get_accel_cal_matrix(long *data);
- inv_error_t inv_get_mag_cal_matrix(long *data);
- inv_error_t inv_get_mag_bias_error(long *data);
- inv_error_t inv_get_mag_scale(long *data);
- inv_error_t inv_get_local_field(long *data);
- inv_error_t inv_get_relative_quaternion(long *data);
- inv_error_t inv_get_gyro_float(float *data);
- inv_error_t inv_get_accel_float(float *data);
- inv_error_t inv_get_temperature_float(float *data);
- inv_error_t inv_get_rot_mat_float(float *data);
- inv_error_t inv_get_quaternion_float(float *data);
- inv_error_t inv_get_linear_accel_float(float *data);
- inv_error_t inv_get_linear_accel_in_world_float(float *data);
- inv_error_t inv_get_gravity_float(float *data);
- inv_error_t inv_get_angular_velocity_float(float *data);
- inv_error_t inv_get_euler_angles_float(float *data);
- inv_error_t inv_get_euler_angles_x_float(float *data);
- inv_error_t inv_get_euler_angles_y_float(float *data);
- inv_error_t inv_get_euler_angles_z_float(float *data);
- inv_error_t inv_get_gyro_temp_slope_float(float *data);
- inv_error_t inv_get_gyro_bias_float(float *data);
- inv_error_t inv_get_accel_bias_float(float *data);
- inv_error_t inv_get_mag_bias_float(float *data);
- inv_error_t inv_get_gyro_and_accel_sensor_float(float *data);
- inv_error_t inv_get_mag_raw_data_float(float *data);
- inv_error_t inv_get_magnetometer_float(float *data);
- inv_error_t inv_get_pressure_float(float *data);
- inv_error_t inv_get_heading_float(float *data);
- inv_error_t inv_get_gyro_cal_matrix_float(float *data);
- inv_error_t inv_get_accel_cal_matrix_float(float *data);
- inv_error_t inv_get_mag_cal_matrix_float(float *data);
- inv_error_t inv_get_mag_bias_error_float(float *data);
- inv_error_t inv_get_mag_scale_float(float *data);
- inv_error_t inv_get_local_field_float(float *data);
- inv_error_t inv_get_relative_quaternion_float(float *data);
- inv_error_t inv_get_compass_accuracy(int *accuracy);
- inv_error_t inv_set_gyro_bias(long *data);
- inv_error_t inv_set_accel_bias(long *data);
- inv_error_t inv_set_mag_bias(long *data);
- inv_error_t inv_set_gyro_temp_slope(long *data);
- inv_error_t inv_set_local_field(long *data);
- inv_error_t inv_set_mag_scale(long *data);
- inv_error_t inv_set_gyro_temp_slope_float(float *data);
- inv_error_t inv_set_gyro_bias_float(float *data);
- inv_error_t inv_set_accel_bias_float(float *data);
- inv_error_t inv_set_mag_bias_float(float *data);
- inv_error_t inv_set_local_field_float(float *data);
- inv_error_t inv_set_mag_scale_float(float *data);
-
- inv_error_t inv_apply_endian_accel(void);
- inv_error_t inv_apply_calibration(void);
- inv_error_t inv_set_gyro_calibration(float range, signed char *orientation);
- inv_error_t inv_set_accel_calibration(float range,
- signed char *orientation);
- inv_error_t inv_set_compass_calibration(float range,
- signed char *orientation);
-
- /*API for detecting change of state */
- inv_error_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);
-
- inv_error_t inv_set_no_motion_time(float time);
- inv_error_t inv_set_no_motion_thresh(float thresh);
- 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);
-
- // Private functions shared accross modules
- void inv_init_ml(void);
-
- inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func);
- inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func);
- void inv_run_dmp_interupt_cb(void);
- void inv_set_mode_change(inv_error_t(*mode_change_func)
- (unsigned long, unsigned long));
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INV_H
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.c b/60xx/mlsdk/mllite/mlBiasNoMotion.c
deleted file mode 100644
index 73321ff..0000000
--- a/60xx/mlsdk/mllite/mlBiasNoMotion.c
+++ /dev/null
@@ -1,393 +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:$
- *
- *****************************************************************************/
-
-#include "mlBiasNoMotion.h"
-#include "ml.h"
-#include "mlinclude.h"
-#include "mlos.h"
-#include "mlFIFO.h"
-#include "dmpKey.h"
-#include "accel.h"
-#include "mlMathFunc.h"
-#include "mldl.h"
-#include "mlstates.h"
-#include "mlSetGyroBias.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-BiasNoMot"
-
-
-#define _mlDebug(x) //{x}
-
-/**
- * @brief inv_set_motion_callback is used to register a callback function that
- * will trigger when a change of motion state is detected.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * and inv_dmp_start()
- * must <b>NOT</b> have been called.
- *
- * @param func A user defined callback function accepting a
- * motion_state parameter, the new motion state.
- * May be one of INV_MOTION or INV_NO_MOTION.
- * @return INV_SUCCESS if successful or Non-zero error code otherwise.
- */
-inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state))
-{
- INVENSENSE_FUNC_START;
-
- if ((inv_get_state() != INV_STATE_DMP_OPENED) &&
- (inv_get_state() != INV_STATE_DMP_STARTED))
- return INV_ERROR_SM_IMPROPER_STATE;
-
- inv_params_obj.motion_cb_func = func;
-
- return INV_SUCCESS;
-}
-
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
-/** Turns on the feature to compute gyro bias from No Motion */
-inv_error_t inv_turn_on_bias_from_no_motion()
-{
- inv_error_t result;
- unsigned char regs[3] = { 0x0d, DINA35, 0x5d };
- inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION;
- result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs);
- return result;
-}
-
-/** Turns off the feature to compute gyro bias from No Motion
-*/
-inv_error_t inv_turn_off_bias_from_no_motion()
-{
- inv_error_t result;
- unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 };
- inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION;
- result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs);
- return result;
-}
-#endif
-
-inv_error_t inv_update_bias(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- unsigned char regs[12];
- short bias[GYRO_NUM_AXES];
-
- if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION)
- && inv_get_gyro_present()) {
-
- regs[0] = DINAA0 + 3;
- result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_get_mpu_memory(KEY_D_1_244, 12, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- inv_convert_bias(regs, bias);
-
- regs[0] = DINAA0 + 15;
- result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result =
- inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
- MPUREG_TEMP_OUT_H, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- inv_obj.got_no_motion_bias = TRUE;
- }
- return INV_SUCCESS;
-}
-
-inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj)
-{
- long gain;
- unsigned long timeChange;
- long rate;
- inv_error_t result;
- long accel[3], temp;
- long long accelMag;
- unsigned long currentTime;
- int kk;
-
- if (!inv_accel_present()) {
- return INV_SUCCESS;
- }
-
- currentTime = inv_get_tick_count();
-
- // We always run the accel low pass filter at the highest sample rate possible
- result = inv_get_accel(accel);
- if (result != INV_ERROR_FEATURE_NOT_ENABLED) {
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- rate = inv_get_fifo_rate() * 5 + 5;
- if (rate > 200)
- rate = 200;
-
- gain = inv_obj->accel_lpf_gain * rate;
- timeChange = inv_get_fifo_rate();
-
- accelMag = 0;
- for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
- inv_obj->accel_lpf[kk] =
- inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]);
- inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]);
- temp = accel[0] - inv_obj->accel_lpf[0];
- accelMag += (long long)temp *temp;
- }
-
- if (accelMag > inv_obj->no_motion_accel_threshold) {
- inv_obj->no_motion_accel_time = currentTime;
-
- // Check for change of state
- 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())
- inv_set_motion_state(INV_NO_MOTION);
- }
- }
- return INV_SUCCESS;
-}
-
-/**
- * @internal
- * @brief Manually update the motion/no motion status. This is a
- * convienence function for implementations that do not wish to use
- * inv_update_data.
- * This function can be called periodically to check for the
- * 'no motion' state and update the internal motion status and bias
- * calculations.
- */
-inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[3] = { 0 };
- unsigned short motionFlag = 0;
- unsigned long currentTime;
- inv_error_t result;
-
- result = MLAccelMotionDetection(inv_obj);
-
- currentTime = inv_get_tick_count();
-
- // If it is not time to poll for a no motion event, return
- if (((inv_obj->interrupt_sources & INV_INT_MOTION) == 0) &&
- ((currentTime - inv_obj->poll_no_motion) <= 1000))
- return INV_SUCCESS;
-
- inv_obj->poll_no_motion = currentTime;
-
-#if defined CONFIG_MPU_SENSORS_MPU3050
- if (inv_get_gyro_present()
- && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) {
- static long repeatBiasUpdateTime = 0;
-
- result = inv_get_mpu_memory(KEY_D_1_98, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- 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) {
- if (inv_obj->motion_state == INV_MOTION) {
- inv_update_bias();
- repeatBiasUpdateTime = inv_get_tick_count();
-
- regs[0] = DINAD8 + 1;
- regs[1] = DINA0C;
- regs[2] = DINAD8 + 2;
- result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- regs[0] = 0;
- regs[1] = 5;
- result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- //Trigger no motion callback
- inv_set_motion_state(INV_NO_MOTION);
- }
- }
- if (motionFlag == 5) {
- if (inv_obj->motion_state == INV_NO_MOTION) {
- regs[0] = DINAD8 + 2;
- regs[1] = DINA0C;
- regs[2] = DINAD8 + 1;
- result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- regs[0] =
- (unsigned char)((inv_obj->motion_duration >> 8) & 0xff);
- regs[1] = (unsigned char)(inv_obj->motion_duration & 0xff);
- result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- //Trigger no motion callback
- inv_set_motion_state(INV_MOTION);
- }
- }
- if (inv_obj->motion_state == INV_NO_MOTION) {
- if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) {
- inv_update_bias();
- repeatBiasUpdateTime = inv_get_tick_count();
- }
- }
- }
-#else // CONFIG_MPU_SENSORS_MPU3050
- if (inv_get_gyro_present()
- && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) {
- result = inv_get_mpu_memory(KEY_D_1_98, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1];
-
- _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag);
- )
- if (motionFlag > 0) {
- unsigned char biasReg[12];
- long biasTmp2[3], biasTmp[3];
- int i;
-
- if (inv_obj->last_motion != motionFlag) {
- result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg);
-
- for (i = 0; i < 3; i++) {
- biasTmp2[i] = inv_big8_to_int32(&biasReg[i * 4]);
- }
- // Rotate bias vector by the transpose of the orientation matrix
- for (i = 0; i < 3; ++i) {
- biasTmp[i] =
- inv_q30_mult(biasTmp2[0],
- inv_obj->gyro_orient[i]) +
- inv_q30_mult(biasTmp2[1],
- inv_obj->gyro_orient[i + 3]) +
- inv_q30_mult(biasTmp2[2], inv_obj->gyro_orient[i + 6]);
- }
- inv_obj->gyro_bias[0] = inv_q30_mult(biasTmp[0], 1501974482L);
- inv_obj->gyro_bias[1] = inv_q30_mult(biasTmp[1], 1501974482L);
- inv_obj->gyro_bias[2] = inv_q30_mult(biasTmp[2], 1501974482L);
- }
- inv_set_motion_state(INV_NO_MOTION);
- } else {
- // We are in a motion state
- inv_set_motion_state(INV_MOTION);
- }
- inv_obj->last_motion = motionFlag;
-
- }
-#endif // CONFIG_MPU_SENSORS_MPU3050
- return INV_SUCCESS;
-}
-
-inv_error_t inv_enable_bias_no_motion(void)
-{
- inv_error_t result;
- inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION;
- result =
- inv_register_fifo_rate_process(MLPollMotionStatus,
- INV_PRIORITY_BIAS_NO_MOTION);
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_turn_on_bias_from_no_motion();
-#endif
- return result;
-}
-
-inv_error_t inv_disable_bias_no_motion(void)
-{
- inv_error_t result;
- inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION;
- result = inv_unregister_fifo_rate_process(MLPollMotionStatus);
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_turn_off_bias_from_no_motion();
-#endif
- return result;
-}
diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.h b/60xx/mlsdk/mllite/mlBiasNoMotion.h
deleted file mode 100644
index 030dbf9..0000000
--- a/60xx/mlsdk/mllite/mlBiasNoMotion.h
+++ /dev/null
@@ -1,40 +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$
- *
- *****************************************************************************/
-
-#ifndef INV_BIAS_NO_MOTION_H__
-#define INV_BIAS_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_enable_bias_no_motion(void);
- inv_error_t inv_disable_bias_no_motion(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INV_BIAS_NO_MOTION_H__
diff --git a/60xx/mlsdk/mllite/mlFIFO.c b/60xx/mlsdk/mllite/mlFIFO.c
deleted file mode 100644
index 2c0d2f0..0000000
--- a/60xx/mlsdk/mllite/mlFIFO.c
+++ /dev/null
@@ -1,2265 +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: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *******************************************************************************/
-
-/**
- * @defgroup MLFIFO
- * @brief Motion Library - FIFO Driver.
- * The FIFO API Interface.
- *
- * @{
- * @file mlFIFO.c
- * @brief FIFO Interface.
-**/
-
-#include <string.h>
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-# include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-# include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-# include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "dmpKey.h"
-#include "mlMathFunc.h"
-#include "ml.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mlstates.h"
-#include "mlsupervisor.h"
-#include "mlos.h"
-#include "mlmath.h"
-#include "accel.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-fifo"
-
-#define FIFO_DEBUG 0
-
-#define REF_QUATERNION (0)
-#define REF_GYROS (REF_QUATERNION + 4)
-#define REF_CONTROL (REF_GYROS + 3)
-#define REF_RAW (REF_CONTROL + 4)
-#define REF_RAW_EXTERNAL (REF_RAW + 8)
-#define REF_ACCEL (REF_RAW_EXTERNAL + 6)
-#define REF_QUANT_ACCEL (REF_ACCEL + 3)
-#define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES)
-#define REF_EIS (REF_QUATERNION_6AXIS + 4)
-#define REF_DMP_PACKET (REF_EIS + 3)
-#define REF_GARBAGE (REF_DMP_PACKET + 1)
-#define REF_LAST (REF_GARBAGE + 1)
-
-long fifo_scale[REF_LAST] = {
- (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion
- // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2)
- 1501974482L, 1501974482L, 1501974482L, // Gyro
- (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control
- (1L << 14), // Temperature
- (1L << 14), (1L << 14), (1L << 14), // Raw Gyro
- (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding
- (1L << 14), (1L << 14), (1L << 14), // Raw External
- (1L << 14), (1L << 14), (1L << 14), // Raw External
- (1L << 16), (1L << 16), (1L << 16), // Accel
- (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel
- (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel
- (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis
- (1L << 30), (1L << 30), (1L << 30), // EIS
- (1L << 30), // Packet
- (1L << 30), // Garbage
-};
-
-// The scale factors for tap need to match the number in fifo_scale above.
-// fifo_base_offset below may also need to be changed if this is not 8
-#if INV_MAX_NUM_ACCEL_SAMPLES != 8
-#error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8
-#endif
-
-#define CONFIG_QUAT (0)
-#define CONFIG_GYROS (CONFIG_QUAT + 1)
-#define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1)
-#define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1)
-#define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1)
-#define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1)
-#define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1)
-#define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1)
-#define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1)
-#define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1)
-#define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1)
-#define NUMFIFOELEMENTS (CONFIG_FOOTER + 1)
-
-const int fifo_base_offset[NUMFIFOELEMENTS] = {
- REF_QUATERNION * 4,
- REF_GYROS * 4,
- REF_CONTROL * 4,
- REF_RAW * 4,
- REF_RAW * 4 + 4,
- REF_RAW_EXTERNAL * 4,
- REF_ACCEL * 4,
- REF_QUANT_ACCEL * 4,
- REF_EIS * 4,
- REF_DMP_PACKET * 4,
- REF_GARBAGE * 4
-};
-
-struct fifo_obj {
- void (*fifo_process_cb) (void);
- long decoded[REF_LAST];
- long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES];
- int offsets[REF_LAST * 4];
- int cache;
- uint_fast8_t gyro_source;
- unsigned short fifo_rate;
- unsigned short sample_step_size_ms;
- uint_fast16_t fifo_packet_size;
- uint_fast16_t data_config[NUMFIFOELEMENTS];
- unsigned char reference_count[REF_LAST];
- long acc_bias_filt[3];
- float acc_filter_coef;
- long gravity_cache[3];
-};
-
-static struct fifo_obj fifo_obj;
-
-#define FIFO_CACHE_TEMPERATURE 1
-#define FIFO_CACHE_GYRO 2
-#define FIFO_CACHE_GRAVITY_BODY 4
-#define FIFO_CACHE_ACC_BIAS 8
-
-struct fifo_rate_obj {
- // These describe callbacks happening everytime a FIFO block is processed
- int_fast8_t num_cb;
- HANDLE mutex;
- inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES];
- int priority[MAX_HIGH_RATE_PROCESSES];
-};
-
-struct fifo_rate_obj fifo_rate_obj;
-
-/** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old
- * accuracy if needed.
- */
-static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements,
- uint_fast16_t accuracy,
- uint_fast8_t configOffset)
-{
- if (elements) {
- if (!accuracy)
- accuracy = fifo_obj.data_config[configOffset];
- else if (accuracy & INV_16_BIT)
- if ((fifo_obj.data_config[configOffset] & INV_32_BIT))
- accuracy = INV_32_BIT; // 32-bits takes priority
- else
- accuracy = INV_16_BIT;
- else
- accuracy = INV_32_BIT;
- } else {
- accuracy = 0;
- }
-
- return accuracy;
-}
-
-/** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0,
- * the reference counts are subtracted, otherwise they are incremented for each
- * bit set in element. The value returned are the elements that should be sent
- * out as data through the FIFO.
-*/
-static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements,
- uint_fast16_t increment,
- uint_fast8_t refOffset,
- uint_fast8_t len)
-{
- uint_fast8_t kk;
-
- if (increment == 0) {
- for (kk = 0; kk < len; ++kk) {
- if ((elements & 1)
- && (fifo_obj.reference_count[kk + refOffset] > 0)) {
- fifo_obj.reference_count[kk + refOffset]--;
- }
- elements >>= 1;
- }
- } else {
- for (kk = 0; kk < len; ++kk) {
- if (elements & 1)
- fifo_obj.reference_count[kk + refOffset]++;
- elements >>= 1;
- }
- }
- elements = 0;
- for (kk = 0; kk < len; ++kk) {
- if (fifo_obj.reference_count[kk + refOffset] > 0)
- elements |= (1 << kk);
- }
- return elements;
-}
-
-/**
- * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send
- * out the FIFO, 0 when removing from the FIFO.
- */
-static inv_error_t inv_construct3_fifo(unsigned char *regs,
- uint_fast16_t elements,
- uint_fast16_t accuracy,
- uint_fast8_t refOffset,
- unsigned short key,
- uint_fast8_t configOffset)
-{
- int_fast8_t kk;
- inv_error_t result;
-
- elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3);
- accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset);
-
- if (accuracy & INV_16_BIT) {
- regs[0] = DINAF8 + 2;
- }
-
- fifo_obj.data_config[configOffset] = elements | accuracy;
-
- for (kk = 0; kk < 3; ++kk) {
- if ((elements & 1) == 0)
- regs[kk + 1] = DINAA0 + 3;
- elements >>= 1;
- }
-
- result = inv_set_mpu_memory(key, 4, regs);
-
- return result;
-}
-
-/**
- * @internal
- * Puts footer on FIFO data.
- */
-static inv_error_t inv_set_footer(void)
-{
- unsigned char regs = DINA30;
- uint_fast8_t tmp_count;
- int_fast8_t i, j;
- int offset;
- int result;
- int *fifo_offsets_ptr = fifo_obj.offsets;
-
- fifo_obj.fifo_packet_size = 0;
- for (i = 0; i < NUMFIFOELEMENTS; i++) {
- tmp_count = 0;
- offset = fifo_base_offset[i];
- for (j = 0; j < 8; j++) {
- if ((fifo_obj.data_config[i] >> j) & 0x0001) {
-#ifndef BIG_ENDIAN
- // Special Case for Byte Ordering on Accel Data
- if ((i == CONFIG_RAW_DATA) && (j > 2)) {
- tmp_count += 2;
- switch (inv_get_dl_config()->accel->endian) {
- case EXT_SLAVE_BIG_ENDIAN:
- *fifo_offsets_ptr++ = offset + 3;
- *fifo_offsets_ptr++ = offset + 2;
- break;
- case EXT_SLAVE_LITTLE_ENDIAN:
- *fifo_offsets_ptr++ = offset + 2;
- *fifo_offsets_ptr++ = offset + 3;
- break;
- case EXT_SLAVE_FS8_BIG_ENDIAN:
- if (j == 3) {
- // Throw this byte away
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- *fifo_offsets_ptr++ = offset + 3;
- } else if (j == 4) {
- *fifo_offsets_ptr++ = offset + 3;
- *fifo_offsets_ptr++ = offset + 7;
- } else {
- // Throw these byte away
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- }
- break;
- case EXT_SLAVE_FS16_BIG_ENDIAN:
- if (j == 3) {
- // Throw this byte away
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- *fifo_offsets_ptr++ = offset + 3;
- } else if (j == 4) {
- *fifo_offsets_ptr++ = offset - 2;
- *fifo_offsets_ptr++ = offset + 3;
- } else {
- *fifo_offsets_ptr++ = offset - 2;
- *fifo_offsets_ptr++ = offset + 3;
- }
- break;
- default:
- return INV_ERROR; // Bad value on ordering
- }
- } else {
- tmp_count += 2;
- *fifo_offsets_ptr++ = offset + 3;
- *fifo_offsets_ptr++ = offset + 2;
- if (fifo_obj.data_config[i] & INV_32_BIT) {
- *fifo_offsets_ptr++ = offset + 1;
- *fifo_offsets_ptr++ = offset;
- tmp_count += 2;
- }
- }
-#else
- // Big Endian Platform
- // Special Case for Byte Ordering on Accel Data
- if ((i == CONFIG_RAW_DATA) && (j > 2)) {
- tmp_count += 2;
- switch (inv_get_dl_config()->accel->endian) {
- case EXT_SLAVE_BIG_ENDIAN:
- *fifo_offsets_ptr++ = offset + 2;
- *fifo_offsets_ptr++ = offset + 3;
- break;
- case EXT_SLAVE_LITTLE_ENDIAN:
- *fifo_offsets_ptr++ = offset + 3;
- *fifo_offsets_ptr++ = offset + 2;
- break;
- case EXT_SLAVE_FS8_BIG_ENDIAN:
- if (j == 3) {
- // Throw this byte away
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- *fifo_offsets_ptr++ = offset;
- } else if (j == 4) {
- *fifo_offsets_ptr++ = offset;
- *fifo_offsets_ptr++ = offset + 4;
- } else {
- // Throw these bytes away
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- }
- break;
- case EXT_SLAVE_FS16_BIG_ENDIAN:
- if (j == 3) {
- // Throw this byte away
- *fifo_offsets_ptr++ =
- fifo_base_offset[CONFIG_FOOTER];
- *fifo_offsets_ptr++ = offset;
- } else if (j == 4) {
- *fifo_offsets_ptr++ = offset - 3;
- *fifo_offsets_ptr++ = offset;
- } else {
- *fifo_offsets_ptr++ = offset - 3;
- *fifo_offsets_ptr++ = offset;
- }
- break;
- default:
- return INV_ERROR; // Bad value on ordering
- }
- } else {
- tmp_count += 2;
- *fifo_offsets_ptr++ = offset;
- *fifo_offsets_ptr++ = offset + 1;
- if (fifo_obj.data_config[i] & INV_32_BIT) {
- *fifo_offsets_ptr++ = offset + 2;
- *fifo_offsets_ptr++ = offset + 3;
- tmp_count += 2;
- }
- }
-
-#endif
- }
- offset += 4;
- }
- fifo_obj.fifo_packet_size += tmp_count;
- }
- if (fifo_obj.data_config[CONFIG_FOOTER] == 0 &&
- fifo_obj.fifo_packet_size > 0) {
- // Add footer
- result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT;
- fifo_obj.fifo_packet_size += 2;
- } else if (fifo_obj.data_config[CONFIG_FOOTER] &&
- (fifo_obj.fifo_packet_size == 2)) {
- // Remove Footer
- regs = DINAA0 + 3;
- result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- fifo_obj.data_config[CONFIG_FOOTER] = 0;
- fifo_obj.fifo_packet_size = 0;
- }
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_decode_quantized_accel(void)
-{
- int kk;
- int fifoRate = inv_get_fifo_rate();
-
- if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1));
- kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) {
- union {
- unsigned int u10:10;
- signed int s10:10;
- } temp;
-
- union {
- uint32_t u32;
- int32_t s32;
- } value;
-
- value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk];
- // unquantize this samples.
- // They are stored as x * 2^20 + y * 2^10 + z
- // Z
- temp.u10 = value.u32 & 0x3ff;
- value.s32 -= temp.s10;
- fifo_obj.decoded_accel[kk][2] = temp.s10 * 64;
- // Y
- value.s32 = value.s32 / 1024;
- temp.u10 = value.u32 & 0x3ff;
- value.s32 -= temp.s10;
- fifo_obj.decoded_accel[kk][1] = temp.s10 * 64;
- // X
- value.s32 = value.s32 / 1024;
- temp.u10 = value.u32 & 0x3ff;
- fifo_obj.decoded_accel[kk][0] = temp.s10 * 64;
- }
- return INV_SUCCESS;
-}
-
-static inv_error_t inv_state_change_fifo(unsigned char newState)
-{
- inv_error_t result = INV_SUCCESS;
- unsigned char regs[4];
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- /* Don't reset the fifo on a fifo rate change */
- if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
- (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) {
- /* Delay output on restart by 50ms due to warm up time of gyros */
-
- short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1);
- inv_init_fifo_hardare();
- inv_int16_to_big8(delay, regs);
- result = inv_set_mpu_memory(KEY_D_1_178, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- if (INV_STATE_DMP_STARTED == newState) {
- if (inv_dmpkey_supported(KEY_D_1_128)) {
- double tmp;
- tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1);
- if (tmp > 0x40000000L)
- tmp = 0x40000000L;
- (void)inv_int32_to_big8((long)tmp, regs);
- result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_reset_fifo();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- }
- return result;
-}
-
-/**
- * @internal
- * @brief get the FIFO packet size
- * @return the FIFO packet size
- */
-uint_fast16_t inv_get_fifo_packet_size(void)
-{
- return fifo_obj.fifo_packet_size;
-}
-
-/**
- * @brief Initializes all the internal static variables for
- * the FIFO module.
- * @note Should be called by the initialization routine such
- * as inv_dmp_open().
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_init_fifo_param(void)
-{
- inv_error_t result;
- memset(&fifo_obj, 0, sizeof(struct fifo_obj));
- fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity
- inv_set_linear_accel_filter_coef(0.f);
- fifo_obj.fifo_rate = 20;
- fifo_obj.sample_step_size_ms = 100;
- memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
- result = inv_create_mutex(&fifo_rate_obj.mutex);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_register_state_callback(inv_state_change_fifo);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return result;
-}
-
-/**
- * @brief Close the FIFO usage.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_close_fifo(void)
-{
- inv_error_t result;
- inv_error_t first = INV_SUCCESS;
- result = inv_unregister_state_callback(inv_state_change_fifo);
- ERROR_CHECK_FIRST(first, result);
- result = inv_destroy_mutex(fifo_rate_obj.mutex);
- ERROR_CHECK_FIRST(first, result);
- memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj));
- return first;
-}
-
-/**
- * Set the gyro source to output to the fifo
- *
- * @param source The source. One of
- * - INV_GYRO_FROM_RAW
- * - INV_GYRO_FROM_QUATERNION
- *
- * @return INV_SUCCESS or non-zero error code;
- */
-inv_error_t inv_set_gyro_data_source(uint_fast8_t source)
-{
- if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- fifo_obj.gyro_source = source;
- return INV_SUCCESS;
-}
-
-/**
- * @brief Reads and processes FIFO data. Also handles callbacks when data is
- * ready.
- * @param numPackets
- * Number of FIFO packets to try to read. You should
- * use a large number here, such as 100, if you want to read all
- * the full packets in the FIFO, which is typical operation.
- * @param processed
- * The number of FIFO packets processed. This may be incremented
- * even if high rate processes later fail.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets,
- int_fast8_t * processed)
-{
- int_fast8_t packet;
- inv_error_t result = INV_SUCCESS;
- uint_fast16_t read;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- int kk;
-
- if (NULL == processed)
- return INV_ERROR_INVALID_PARAMETER;
-
- *processed = 0;
- if (fifo_obj.fifo_packet_size == 0)
- return result; // Nothing to read
-
- for (packet = 0; packet < numPackets; ++packet) {
- if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
- unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE];
- unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE];
- read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size,
- footer_n_data);
- if (0 == read ||
- read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) {
- result = inv_get_fifo_status();
- if (INV_SUCCESS != result) {
- memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
- }
- return result;
- }
-
- result = inv_process_fifo_packet(buf);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else if (inv_accel_present()) {
- long data[ACCEL_NUM_AXES];
- result = inv_get_accel_data(data);
- if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
- return INV_SUCCESS;
- }
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
- fifo_obj.cache = 0;
- for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
- fifo_obj.decoded[REF_RAW + 4 + kk] =
- inv_q30_mult((data[kk] << 16),
- fifo_scale[REF_RAW + 4 + kk]);
- fifo_obj.decoded[REF_ACCEL + kk] =
- inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]);
- fifo_obj.decoded[REF_ACCEL + kk] -=
- inv_obj.scaled_accel_bias[kk];
- }
- }
- // The buffer was processed correctly, so increment even if
- // other processes fail later, which will return an error
- *processed = *processed + 1;
-
- if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) &&
- fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) {
- result = inv_decode_quantized_accel();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- if (fifo_obj.data_config[CONFIG_QUAT]) {
- result = inv_accel_compass_supervisor();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- 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) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- }
- return result;
-}
-
-/**
- * @brief inv_set_fifo_processed_callback is used to set a processed data callback
- * function. inv_set_fifo_processed_callback sets a user defined callback
- * function that triggers when all the decoding has been finished by
- * the motion processing engines. It is called before other bigger
- * processing engines to allow lower latency for the user.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * and inv_dmp_start()
- * must <b>NOT</b> have been called.
- *
- * @param func A user defined callback function.
- *
- * @return INV_SUCCESS if successful, or non-zero error code otherwise.
- */
-inv_error_t inv_set_fifo_processed_callback(void (*func) (void))
-{
- INVENSENSE_FUNC_START;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- fifo_obj.fifo_process_cb = func;
-
- return INV_SUCCESS;
-}
-
-/**
- * @internal
- * @brief Process data from the dmp read via the fifo. Takes a buffer
- * filled with bytes read from the DMP FIFO.
- * Currently expects only the 16 bytes of quaternion data.
- * Calculates the motion parameters from that data and stores the
- * results in an internal data structure.
- * @param[in,out] dmpData Pointer to the buffer containing the fifo data.
- * @return INV_SUCCESS or error code.
-**/
-inv_error_t inv_process_fifo_packet(const unsigned char *dmpData)
-{
- INVENSENSE_FUNC_START;
- int N, kk;
- unsigned char *p;
-
- p = (unsigned char *)(&fifo_obj.decoded);
- N = fifo_obj.fifo_packet_size;
- if (N > sizeof(fifo_obj.decoded))
- return INV_ERROR_ASSERTION_FAILURE;
-
- memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded));
-
- for (kk = 0; kk < N; ++kk) {
- p[fifo_obj.offsets[kk]] = *dmpData++;
- }
-
- // If multiplies are much greater cost than if checks, you could check
- // to see if fifo_scale is non-zero first, or equal to (1L<<30)
- for (kk = 0; kk < REF_LAST; ++kk) {
- fifo_obj.decoded[kk] =
- inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]);
- }
-
- memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS],
- &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long));
-
- inv_obj.flags[INV_PROCESSED_DATA_READY] = 1;
- fifo_obj.cache = 0;
-
- return INV_SUCCESS;
-}
-
-/** Converts 16-bit temperature data as read from temperature register
-* into Celcius scaled by 2^16.
-*/
-long inv_decode_temperature(short tempReg)
-{
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
- // Celcius = 35 + (T + 3048.7)/325.9
- return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L);
-#endif
-#if defined CONFIG_MPU_SENSORS_MPU6050B1
- // Celcius = 35 + (T + 927.4)/360.6
- return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L);
-#endif
-#if defined CONFIG_MPU_SENSORS_MPU3050
- // Celcius = 35 + (T + 13200)/280
- return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L);
-#endif
-}
-
-/** @internal
-* Returns the temperature in hardware units. The scaling may change from part to part.
-*/
-inv_error_t inv_get_temperature_raw(short *data)
-{
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) {
- inv_error_t result;
- unsigned char regs[2];
- if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) {
- if (FIFO_DEBUG)
- MPL_LOGI("Fetching the temperature from the registers\n");
- fifo_obj.cache |= FIFO_CACHE_TEMPERATURE;
- result = inv_serial_read(inv_get_serial_handle(),
- inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2,
- regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]);
- }
- }
- *data = (short)fifo_obj.decoded[REF_RAW];
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 1-element vector of temperature. It is read from the hardware if it
- * doesn't exist in the FIFO.
- * @param[out] data 1-element vector of temperature
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_temperature(long *data)
-{
- short tr;
- inv_error_t result;
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
- result = inv_get_temperature_raw(&tr);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- data[0] = inv_decode_temperature(tr);
- return INV_SUCCESS;
-}
-
-/**
- * @brief Get the Decoded Accel Data.
- * @param data
- * a buffer to store the quantized data.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_get_unquantized_accel(long *data)
-{
- int ii, kk;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
- for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
- data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk];
- }
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Get the Quantized Accel data algorithm output from the FIFO.
- * @param data
- * a buffer to store the quantized data.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_get_quantized_accel(long *data)
-{
- int ii;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) {
- data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii];
- }
-
- return INV_SUCCESS;
-}
-
-/** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO
-* and it is read from the registers if it was not put into the FIFO. The data is
-* cached till the next FIFO processing block time.
-* @param[out] data Length 3, Gyro data
-*/
-inv_error_t inv_get_gyro_sensor(long *data)
-{
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
- if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) {
- inv_error_t result;
- unsigned char regs[6];
- if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) {
- fifo_obj.cache |= FIFO_CACHE_GYRO;
- result =
- inv_serial_read(inv_get_serial_handle(),
- inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6,
- regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- fifo_obj.decoded[REF_RAW + 1] =
- (((long)regs[0]) << 24) | (((long)regs[1]) << 16);
- fifo_obj.decoded[REF_RAW + 2] =
- (((long)regs[2]) << 24) | (((long)regs[3]) << 16);
- fifo_obj.decoded[REF_RAW + 3] =
- (((long)regs[4]) << 24) | (((long)regs[5]) << 16);
-
- // Temperature starts at location 0, Gyro at location 1.
- fifo_obj.decoded[REF_RAW + 1] =
- inv_q30_mult(fifo_obj.decoded[REF_RAW + 1],
- fifo_scale[REF_RAW + 1]);
- fifo_obj.decoded[REF_RAW + 2] =
- inv_q30_mult(fifo_obj.decoded[REF_RAW + 2],
- fifo_scale[REF_RAW + 2]);
- fifo_obj.decoded[REF_RAW + 3] =
- inv_q30_mult(fifo_obj.decoded[REF_RAW + 3],
- fifo_scale[REF_RAW + 3]);
- }
- data[0] = fifo_obj.decoded[REF_RAW + 1];
- data[1] = fifo_obj.decoded[REF_RAW + 2];
- data[2] = fifo_obj.decoded[REF_RAW + 3];
- } else {
- long data2[6];
- inv_get_gyro_and_accel_sensor(data2);
- data[0] = data2[0];
- data[1] = data2[1];
- data[2] = data2[2];
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 6-element vector of gyro and accel data
- * @param[out] data 6-element vector of gyro and accel data
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_gyro_and_accel_sensor(long *data)
-{
- int ii;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_RAW_DATA])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) {
- data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii];
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 3-element vector of external sensor
- * @param[out] data 3-element vector of external sensor
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_external_sensor_data(long *data, int size)
-{
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- int ii;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (ii = 0; ii < size && ii < 6; ii++) {
- data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii];
- }
-
- return INV_SUCCESS;
-#else
- memset(data, 0, COMPASS_NUM_AXES * sizeof(long));
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-#endif
-}
-
-/**
- * Sends accelerometer data to the FIFO.
- *
- * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis
- * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- * for a subset.
- *
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 };
- inv_error_t result;
- int kk;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL,
- KEY_CFG_12, CONFIG_ACCEL);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- for (kk = 0; kk < ACCEL_NUM_AXES; kk++) {
- fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens;
- }
-
- return inv_set_footer();
-}
-
-/**
- * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits.
- *
- * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all
- * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd
- * together for a subset.
- *
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- int_fast8_t kk;
- inv_error_t result;
- unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4);
- accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA);
-
- if (accuracy & INV_16_BIT) {
- regs[0] = DINAF8 + 2;
- }
-
- fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy;
-
- for (kk = 0; kk < 4; ++kk) {
- if ((elements & 1) == 0)
- regs[kk + 1] = DINAA0 + 3;
- elements >>= 1;
- }
-
- result = inv_set_mpu_memory(KEY_CFG_1, 5, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-}
-
-/**
- * Adds a rolling counter to the fifo packet. When used with the footer
- * the data comes out the first time:
- *
- * @code
- * <data0><data1>...<dataN><PacketNum0><PacketNum1>
- * @endcode
- * for every other packet it is
- *
- * @code
- * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1>
- * @endcode
- *
- * This allows for scanning of the fifo for packets
- *
- * @return INV_SUCCESS or error code
- */
-inv_error_t inv_send_packet_number(uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- unsigned char regs;
- uint_fast16_t elements;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1);
- if (elements & 1) {
- regs = DINA28;
- fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] =
- INV_ELEMENT_1 | INV_16_BIT;
- } else {
- regs = DINAF8 + 3;
- fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0;
- }
- result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-}
-
-/**
- * @brief Send the computed gravity vectors into the FIFO.
- * The gravity vectors can be retrieved from the FIFO via
- * inv_get_gravity(), to have the gravitation vector expressed
- * in coordinates relative to the body.
- *
- * Gravity is a derived vector derived from the quaternion.
- * @param elements
- * the gravitation vectors components bitmask.
- * To send all compoents use INV_ALL.
- * @param accuracy
- * The number of bits the gravitation vector is expressed
- * into.
- * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data.
- * Set to zero to remove it from the FIFO.
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
-
- result = inv_send_quaternion(accuracy);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-}
-
-/** Sends gyro data to the FIFO. Gyro data is a 3 length vector
- * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
- * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
- * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- * for a subset.
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 };
- inv_error_t result;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) {
- regs[0] = DINA90 + 5;
- result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- regs[0] = DINAF8 + 1;
- regs[1] = DINA20;
- regs[2] = DINA28;
- regs[3] = DINA30;
- } else {
- regs[0] = DINA90 + 10;
- result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- regs[0] = DINAF8 + 1;
- regs[1] = DINA28;
- regs[2] = DINA30;
- regs[3] = DINA38;
- }
- result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS,
- KEY_CFG_9, CONFIG_GYROS);
-
- return inv_set_footer();
-}
-
-/** Sends linear accelerometer data to the FIFO.
- *
- * Linear accelerometer data is a 3 length vector of 32-bits. It is the
- * acceleration in the body frame with gravity removed.
- *
- *
- * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of
- * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- * for a subset.
- *
- * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data. Set to zero to remove it from the FIFO.
- */
-inv_error_t inv_send_linear_accel(uint_fast16_t elements,
- uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- unsigned char state = inv_get_state();
-
- if (state < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- result = inv_send_gravity(elements, accuracy);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_send_accel(elements, accuracy);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-}
-
-/** Sends linear world accelerometer data to the FIFO. Linear world
- * accelerometer data is a 3 length vector of 32-bits. It is the acceleration
- * in the world frame with gravity removed. Should be called once after
- * inv_dmp_open() and before inv_dmp_start().
- *
- * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of
- * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- * for a subset.
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data.
- */
-inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements,
- uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
-
- result = inv_send_linear_accel(INV_ALL, accuracy);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_send_quaternion(accuracy);
-
- return inv_set_footer();
-}
-
-/** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector
- * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start().
- * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16
- * bit data.
- */
-inv_error_t inv_send_quaternion(uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
- DINA30, DINA38
- };
- uint_fast16_t elements, kk;
- inv_error_t result;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4);
- accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT);
-
- if (accuracy & INV_16_BIT) {
- regs[0] = DINAF8 + 2;
- }
-
- fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy;
-
- for (kk = 0; kk < 4; ++kk) {
- if ((elements & 1) == 0)
- regs[kk + 1] = DINAA0 + 3;
- elements >>= 1;
- }
-
- result = inv_set_mpu_memory(KEY_CFG_8, 5, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-}
-
-/** Sends raw data to the FIFO.
- * Should be called once after inv_dmp_open() and before inv_dmp_start().
- * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them
- * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together
- * for a subset. The first element is temperature, the next 3 are gyro data,
- * and the last 3 accel data.
- * @param accuracy
- * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off.
- * @return 0 if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy)
-{
- int result;
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
- DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
- DINAA0 + 3
- };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (accuracy)
- accuracy = INV_16_BIT;
-
- elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
-
- if (elements & 1)
- fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
- else
- fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
- if (elements & 0x7e)
- fifo_obj.data_config[CONFIG_RAW_DATA] =
- (0x3f & (elements >> 1)) | INV_16_BIT;
- else
- fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
-
- if (elements & INV_ELEMENT_1) {
- regs[0] = DINACA;
- }
- if (elements & INV_ELEMENT_2) {
- regs[1] = DINBC4;
- }
- if (elements & INV_ELEMENT_3) {
- regs[2] = DINACC;
- }
- if (elements & INV_ELEMENT_4) {
- regs[3] = DINBC6;
- }
- if (elements & INV_ELEMENT_5) {
- regs[4] = DINBC0;
- }
- if (elements & INV_ELEMENT_6) {
- regs[5] = DINAC8;
- }
- if (elements & INV_ELEMENT_7) {
- regs[6] = DINBC2;
- }
- result = inv_set_mpu_memory(KEY_CFG_15, 7, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-
-#else
- INVENSENSE_FUNC_START;
- unsigned char regs[4] = { DINAA0 + 3,
- DINAA0 + 3,
- DINAA0 + 3,
- DINAA0 + 3
- };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (accuracy)
- accuracy = INV_16_BIT;
-
- elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7);
-
- if (elements & 0x03) {
- elements |= 0x03;
- regs[0] = DINA20;
- }
- if (elements & 0x0C) {
- elements |= 0x0C;
- regs[1] = DINA28;
- }
- if (elements & 0x30) {
- elements |= 0x30;
- regs[2] = DINA30;
- }
- if (elements & 0x40) {
- elements |= 0xC0;
- regs[3] = DINA38;
- }
-
- result = inv_set_mpu_memory(KEY_CFG_15, 4, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- if (elements & 0x01)
- fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT;
- else
- fifo_obj.data_config[CONFIG_TEMPERATURE] = 0;
- if (elements & 0xfe)
- fifo_obj.data_config[CONFIG_RAW_DATA] =
- (0x7f & (elements >> 1)) | INV_16_BIT;
- else
- fifo_obj.data_config[CONFIG_RAW_DATA] = 0;
-
- return inv_set_footer();
-#endif
-}
-
-/** Sends raw external data to the FIFO.
- * Should be called once after inv_dmp_open() and before inv_dmp_start().
- * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them
- * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together
- * for a subset.
- * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data.
- * Sending and Stop sending are reference counted, so data actually
- * stops when the reference reaches zero.
- */
-inv_error_t inv_send_external_sensor_data(uint_fast16_t elements,
- uint_fast16_t accuracy)
-{
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- int result;
- unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3,
- DINAA0 + 3, DINAA0 + 3,
- DINAA0 + 3, DINAA0 + 3 };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (accuracy)
- accuracy = INV_16_BIT;
-
- elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6);
-
- if (elements)
- fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT;
- else
- fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0;
-
- if (elements & INV_ELEMENT_1) {
- regs[0] = DINBC2;
- }
- if (elements & INV_ELEMENT_2) {
- regs[1] = DINACA;
- }
- if (elements & INV_ELEMENT_3) {
- regs[2] = DINBC4;
- }
- if (elements & INV_ELEMENT_4) {
- regs[3] = DINBC0;
- }
- if (elements & INV_ELEMENT_5) {
- regs[4] = DINAC8;
- }
- if (elements & INV_ELEMENT_6) {
- regs[5] = DINACC;
- }
-
- result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-
-#else
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported
-#endif
-}
-
-/**
- * @brief Send the Quantized Acceleromter data into the FIFO. The data can be
- * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel().
- *
- * To be useful this should be set to fifo_rate + 1 if less than
- * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work.
- *
- * @param elements
- * the components bitmask.
- * To send all compoents use INV_ALL.
- *
- * @param accuracy
- * Use INV_32_BIT for 32-bit data or INV_16_BIT for
- * 16-bit data.
- * Set to zero to remove it from the FIFO.
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_send_quantized_accel(uint_fast16_t elements,
- uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28,
- DINA30, DINA38
- };
- unsigned char regs2[4] = { DINA20, DINA28,
- DINA30, DINA38
- };
- inv_error_t result;
- int_fast8_t kk;
- int_fast8_t ii;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8);
-
- if (elements) {
- fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT;
- } else {
- fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0;
- }
-
- for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) {
- fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0;
- for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
- fifo_obj.decoded_accel[kk][ii] = 0;
- }
- }
-
- for (kk = 0; kk < 4; ++kk) {
- if ((elements & 1) == 0)
- regs[kk + 1] = DINAA0 + 3;
- elements >>= 1;
- }
-
- result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- for (kk = 0; kk < 4; ++kk) {
- if ((elements & 1) == 0)
- regs2[kk] = DINAA0 + 3;
- elements >>= 1;
- }
-
- result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return inv_set_footer();
-}
-
-inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy)
-{
- INVENSENSE_FUNC_START;
- int_fast8_t kk;
- unsigned char regs[3] = { DINA28, DINA30, DINA38 };
- inv_error_t result;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (accuracy) {
- accuracy = INV_32_BIT;
- }
-
- elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3);
- accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS);
-
- fifo_obj.data_config[CONFIG_EIS] = elements | accuracy;
-
- for (kk = 0; kk < 3; ++kk) {
- if ((elements & 1) == 0)
- regs[kk] = DINAA0 + 7;
- elements >>= 1;
- }
-
- result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs);
-
- return inv_set_footer();
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer data in body frame.
- *
- * @param[out] data 3-element vector of accelerometer data in body frame.
- * One gee = 2^16.
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_accel(long *data)
-{
- int kk;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if ((!fifo_obj.data_config[CONFIG_ACCEL] &&
- (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR))
- ||
- (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) &&
- !inv_accel_present()))
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
- data[kk] = fifo_obj.decoded[REF_ACCEL + kk];
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 4-element quaternion vector derived from 6-axis or
- * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is
- * gyros, accel and compass.
- *
- * @param[out] data 4-element quaternion vector. One is scaled to 2^30.
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_quaternion(long *data)
-{
- int kk;
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_QUAT])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = 0; kk < 4; ++kk) {
- data[kk] = fifo_obj.decoded[REF_QUATERNION + kk];
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 4-element quaternion vector derived from 6
- * axis sensors (gyros and accels).
- * @param[out] data
- * 4-element quaternion vector. One is scaled to 2^30.
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_6axis_quaternion(long *data)
-{
- int kk;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_QUAT])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = 0; kk < 4; ++kk) {
- data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk];
- }
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_get_relative_quaternion(long *data)
-{
- if (data == NULL)
- return INV_ERROR;
- data[0] = inv_obj.relative_quat[0];
- data[1] = inv_obj.relative_quat[1];
- data[2] = inv_obj.relative_quat[2];
- data[3] = inv_obj.relative_quat[3];
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 3-element vector of gyro data in body frame.
- * @param[out] data
- * 3-element vector of gyro data in body frame
- * with gravity removed. One degree per second = 2^16.
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_gyro(long *data)
-{
- int kk;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (fifo_obj.data_config[CONFIG_GYROS]) {
- for (kk = 0; kk < 3; ++kk) {
- data[kk] = fifo_obj.decoded[REF_GYROS + kk];
- }
- return INV_SUCCESS;
- } else {
- return INV_ERROR_FEATURE_NOT_ENABLED;
- }
-}
-
-/**
- * @brief Get the 3-element gravity vector from the FIFO expressed
- * in coordinates relative to the body frame.
- * @param data
- * 3-element vector of gravity in body frame.
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_gravity(long *data)
-{
- long quat[4];
- int ii;
- inv_error_t result;
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_QUAT])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) {
- fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY;
-
- // Compute it from Quaternion
- result = inv_get_quaternion(quat);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- data[0] =
- inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
- data[1] =
- inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
- data[2] =
- (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) -
- 1073741824L;
-
- for (ii = 0; ii < ACCEL_NUM_AXES; ii++) {
- data[ii] >>= 14;
- fifo_obj.gravity_cache[ii] = data[ii];
- }
- } else {
- data[0] = fifo_obj.gravity_cache[0];
- data[1] = fifo_obj.gravity_cache[1];
- data[2] = fifo_obj.gravity_cache[2];
- }
-
- return INV_SUCCESS;
-}
-
-/**
-* @brief Sets the filter coefficent used for computing the acceleration
-* bias which is used to compute linear acceleration.
-* @param[in] coef Fitler coefficient. 0. means no filter, a small number means
-* a small cutoff frequency with an increasing number meaning
-* an increasing cutoff frequency.
-*/
-inv_error_t inv_set_linear_accel_filter_coef(float coef)
-{
- fifo_obj.acc_filter_coef = coef;
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer data in body frame
- * with gravity removed.
- * @param[out] data 3-element vector of accelerometer data in body frame
- * with gravity removed. One g = 2^16.
- * @return 0 on success or an error code. data unchanged on error.
- */
-inv_error_t inv_get_linear_accel(long *data)
-{
- int kk;
- long grav[3];
- long la[3];
- inv_error_t result;
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- result = inv_get_gravity(grav);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_get_accel(la);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) {
- fifo_obj.cache |= FIFO_CACHE_ACC_BIAS;
-
- for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
- long x;
- x = la[kk] - grav[kk];
- fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef +
- fifo_obj.acc_bias_filt[kk] *
- (1.f -
- fifo_obj.acc_filter_coef));
- data[kk] = x - fifo_obj.acc_bias_filt[kk];
- }
- } else {
- for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
- data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk];
- }
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer data in world frame
- * with gravity removed.
- * @param[out] data 3-element vector of accelerometer data in world frame
- * with gravity removed. One g = 2^16.
- * @return 0 on success or an error code.
- */
-inv_error_t inv_get_linear_accel_in_world(long *data)
-{
- int kk;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
- if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) {
- long wtemp[4], qi[4], wtemp2[4];
- wtemp[0] = 0;
- inv_get_linear_accel(&wtemp[1]);
- inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2);
- inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi);
- inv_q_mult(wtemp2, qi, wtemp);
- for (kk = 0; kk < 3; ++kk) {
- data[kk] = wtemp[kk + 1];
- }
- return INV_SUCCESS;
- } else {
- return INV_ERROR_FEATURE_NOT_ENABLED;
- }
-}
-
-/**
- * @brief Returns 4-element vector of control data.
- * @param[out] data 4-element vector of control data.
- * @return 0 for succes or an error code.
- */
-inv_error_t inv_get_cntrl_data(long *data)
-{
- int kk;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_CONTROL_DATA])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = 0; kk < 4; ++kk) {
- data[kk] = fifo_obj.decoded[REF_CONTROL + kk];
- }
-
- return INV_SUCCESS;
-
-}
-
-/**
- * @brief Returns 3-element vector of EIS shfit data
- * @param[out] data 3-element vector of EIS shift data.
- * @return 0 for succes or an error code.
- */
-inv_error_t inv_get_eis(long *data)
-{
- int kk;
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_EIS])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = 0; kk < 3; ++kk) {
- data[kk] = fifo_obj.decoded[REF_EIS + kk];
- }
-
- return INV_SUCCESS;
-
-}
-
-/**
- * @brief Returns 3-element vector of accelerometer data in body frame.
- * @param[out] data 3-element vector of accelerometer data in body frame in g's.
- * @return 0 for success or an error code.
- */
-inv_error_t inv_get_accel_float(float *data)
-{
- long lData[3];
- int kk;
- int result;
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- result = inv_get_accel(lData);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) {
- data[kk] = lData[kk] / 65536.0f;
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Returns 4-element quaternion vector.
- * @param[out] data 4-element quaternion vector.
- * @return 0 on success, an error code otherwise.
- */
-inv_error_t inv_get_quaternion_float(float *data)
-{
- int kk;
-
- if (data == NULL)
- return INV_ERROR_INVALID_PARAMETER;
-
- if (!fifo_obj.data_config[CONFIG_QUAT])
- return INV_ERROR_FEATURE_NOT_ENABLED;
-
- for (kk = 0; kk < 4; ++kk) {
- data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f;
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Command the MPU to put data in the FIFO at a particular rate.
- *
- * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For
- * example if the MPU is running at 200Hz the following values apply:
- *
- * <TABLE>
- * <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR>
- * <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR>
- * <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR>
- * <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR>
- * <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR>
- * <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR>
- * <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR>
- * </TABLE>
- *
- * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED)
- * then inv_run_state_callbacks() will be called to allow features
- * that depend upon fundamental constants to be updated.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * and inv_dmp_start()
- * must <b>NOT</b> have been called.
- *
- * @param fifoRate Divider value - 1. Output rate is
- * (DMP Sample Rate) / (fifoRate + 1).
- *
- * @return INV_SUCCESS if successful, ML error code on any failure.
- */
-inv_error_t inv_set_fifo_rate(unsigned short fifoRate)
-{
- INVENSENSE_FUNC_START;
- unsigned char regs[2];
- unsigned char state;
- inv_error_t result = INV_SUCCESS;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- state = inv_get_state();
- if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- fifo_obj.fifo_rate = fifoRate;
-
- if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
-
- regs[0] = (unsigned char)((fifoRate >> 8) & 0xff);
- regs[1] = (unsigned char)(fifoRate & 0xff);
-
- result = inv_set_mpu_memory(KEY_D_0_22, 2, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- fifo_obj.sample_step_size_ms =
- (unsigned short)(((long)fifoRate + 1) *
- (inv_mpu_get_sampling_period_us
- (mldl_cfg)) / 1000L);
-
- if (state == INV_STATE_DMP_STARTED)
- result = inv_run_state_callbacks(state);
- } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) {
- struct ext_slave_config config;
- long data;
- config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
- config.len = sizeof(long);
- config.apply = (state == INV_STATE_DMP_STARTED);
- config.data = &data;
- data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1);
-
- /* Ask for the same frequency */
- result = inv_mpu_config_accel(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_mpu_get_accel_config(mldl_cfg,
- inv_get_serial_handle(),
- inv_get_serial_handle(), &config);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- if(FIFO_DEBUG)
- MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000);
- /* Record the actual frequency granted odr is in mHz */
- fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data);
- }
- return result;
-}
-
-/**
- * @brief Retrieve the current FIFO update divider - 1.
- * See inv_set_fifo_rate() for how this value is used.
- *
- * The fifo rate when there is no fifo is the equivilent divider when
- * derived from the value set by SetSampleSteSizeMs()
- *
- * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error.
- */
-unsigned short inv_get_fifo_rate(void)
-{
- return fifo_obj.fifo_rate;
-}
-
-/**
- * @brief Returns the step size for quaternion type data.
- *
- * Typically the data rate for each FIFO packet. When the gryos are sleeping
- * this value will return the last value set by SetSampleStepSizeMs()
- *
- * @return step size for quaternion type data
- */
-int_fast16_t inv_get_sample_step_size_ms(void)
-{
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
- return (fifo_obj.fifo_rate + 1) *
- (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000);
- else
- return fifo_obj.sample_step_size_ms;
-}
-
-/**
- * @brief Returns the step size for quaternion type data.
- *
- * Typically the data rate for each FIFO packet. When the gryos are sleeping
- * this value will return the last value set by SetSampleStepSizeMs()
- *
- * @return step size for quaternion type data
- */
-int_fast16_t inv_get_sample_frequency(void)
-{
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
- return (inv_mpu_get_sampling_rate_hz(mldl_cfg) /
- (fifo_obj.fifo_rate + 1));
- else
- return (1000 / fifo_obj.sample_step_size_ms);
-}
-
-/**
- * @brief The gyro data magnitude squared :
- * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
- * @return the computed magnitude squared output of the gyroscope.
- */
-unsigned long inv_get_gyro_sum_of_sqr(void)
-{
- unsigned long gmag = 0;
- long temp;
- int kk;
-
- for (kk = 0; kk < 3; ++kk) {
- temp = fifo_obj.decoded[REF_GYROS + kk] >>
- (16 - (GYRO_MAG_SQR_SHIFT / 2));
- gmag += temp * temp;
- }
-
- return gmag;
-}
-
-/**
- * @brief The gyro data magnitude squared:
- * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT.
- * @return the computed magnitude squared output of the accelerometer.
- */
-unsigned long inv_accel_sum_of_sqr(void)
-{
- unsigned long amag = 0;
- long temp;
- int kk;
- long accel[3];
- inv_error_t result;
-
- result = inv_get_accel(accel);
- if (INV_SUCCESS != result) {
- return 0;
- }
-
- for (kk = 0; kk < 3; ++kk) {
- temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2));
- amag += temp * temp;
- }
- return amag;
-}
-
-/**
- * @internal
- */
-void inv_override_quaternion(float *q)
-{
- int kk;
- for (kk = 0; kk < 4; ++kk) {
- fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30));
- }
-}
-
-/**
- * @internal
- * @brief This registers a function to be called for each set of
- * gyro/quaternion/rotation matrix/etc output.
- * @param[in] func The callback function to register
- * @param[in] priority The unique priority number of the callback. Lower numbers
- * are called first.
- * @return error code.
- */
-inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- int kk, nn;
-
- result = inv_lock_mutex(fifo_rate_obj.mutex);
- if (INV_SUCCESS != result) {
- return result;
- }
- // Make sure we haven't registered this function already
- // Or used the same priority
- for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
- if ((fifo_rate_obj.fifo_process_cb[kk] == func) ||
- (fifo_rate_obj.priority[kk] == priority)) {
- inv_unlock_mutex(fifo_rate_obj.mutex);
- return INV_ERROR_INVALID_PARAMETER;
- }
- }
-
- // Make sure we have not filled up our number of allowable callbacks
- if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) {
- kk = 0;
- if (fifo_rate_obj.num_cb != 0) {
- // set kk to be where this new callback goes in the array
- while ((kk < fifo_rate_obj.num_cb) &&
- (fifo_rate_obj.priority[kk] < priority)) {
- kk++;
- }
- if (kk != fifo_rate_obj.num_cb) {
- // We need to move the others
- for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) {
- fifo_rate_obj.fifo_process_cb[nn] =
- fifo_rate_obj.fifo_process_cb[nn - 1];
- fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1];
- }
- }
- }
- // Add new callback
- fifo_rate_obj.fifo_process_cb[kk] = func;
- fifo_rate_obj.priority[kk] = priority;
- fifo_rate_obj.num_cb++;
- } else {
- result = INV_ERROR_MEMORY_EXAUSTED;
- }
-
- inv_unlock_mutex(fifo_rate_obj.mutex);
- return result;
-}
-
-/**
- * @internal
- * @brief This unregisters a function to be called for each set of
- * gyro/quaternion/rotation matrix/etc output.
- * @return error code.
- */
-inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func)
-{
- INVENSENSE_FUNC_START;
- int kk, jj;
- inv_error_t result;
-
- result = inv_lock_mutex(fifo_rate_obj.mutex);
- if (INV_SUCCESS != result) {
- return result;
- }
- // Make sure we haven't registered this function already
- result = INV_ERROR_INVALID_PARAMETER;
- for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
- if (fifo_rate_obj.fifo_process_cb[kk] == func) {
- for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) {
- fifo_rate_obj.fifo_process_cb[jj - 1] =
- fifo_rate_obj.fifo_process_cb[jj];
- fifo_rate_obj.priority[jj - 1] =
- fifo_rate_obj.priority[jj];
- }
- fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL;
- fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0;
- fifo_rate_obj.num_cb--;
- result = INV_SUCCESS;
- break;
- }
- }
-
- inv_unlock_mutex(fifo_rate_obj.mutex);
- return result;
-
-}
-#ifdef UMPL
-bool bFIFIDataAvailable = FALSE;
-bool isUmplDataInFIFO(void)
-{
- return bFIFIDataAvailable;
-}
-void setUmplDataInFIFOFlag(bool flag)
-{
- bFIFIDataAvailable = flag;
-}
-#endif
-inv_error_t inv_run_fifo_rate_processes(void)
-{
- int kk;
- inv_error_t result, result2;
-
- result = inv_lock_mutex(fifo_rate_obj.mutex);
- if (INV_SUCCESS != result) {
- MPL_LOGE("MLOsLockMutex returned %d\n", result);
- return result;
- }
- // User callbacks take priority over the fifo_process_cb callback
- if (fifo_obj.fifo_process_cb)
- fifo_obj.fifo_process_cb();
-
- for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) {
- if (fifo_rate_obj.fifo_process_cb[kk]) {
- result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj);
- if (result == INV_SUCCESS)
-#ifdef UMPL
- setUmplDataInFIFOFlag(TRUE);
-#endif
- result = result2;
- }
- }
-
- inv_unlock_mutex(fifo_rate_obj.mutex);
- return result;
-}
-
-/*********************/
- /** \}*//* defgroup */
-/*********************/
diff --git a/60xx/mlsdk/mllite/mlFIFO.h b/60xx/mlsdk/mllite/mlFIFO.h
deleted file mode 100644
index 2114eb3..0000000
--- a/60xx/mlsdk/mllite/mlFIFO.h
+++ /dev/null
@@ -1,150 +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.
- $
- */
-
-#ifndef INVENSENSE_INV_FIFO_H__
-#define INVENSENSE_INV_FIFO_H__
-
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "ml.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlFIFO_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /**************************************************************************/
- /* Elements */
- /**************************************************************************/
-
-#define INV_ELEMENT_1 (0x0001)
-#define INV_ELEMENT_2 (0x0002)
-#define INV_ELEMENT_3 (0x0004)
-#define INV_ELEMENT_4 (0x0008)
-#define INV_ELEMENT_5 (0x0010)
-#define INV_ELEMENT_6 (0x0020)
-#define INV_ELEMENT_7 (0x0040)
-#define INV_ELEMENT_8 (0x0080)
-
-#define INV_ALL (0xFFFF)
-#define INV_ELEMENT_MASK (0x00FF)
-
- /**************************************************************************/
- /* Accuracy */
- /**************************************************************************/
-
-#define INV_16_BIT (0x0100)
-#define INV_32_BIT (0x0200)
-#define INV_ACCURACY_MASK (0x0300)
-
- /**************************************************************************/
- /* Accuracy */
- /**************************************************************************/
-
-#define INV_GYRO_FROM_RAW (0x00)
-#define INV_GYRO_FROM_QUATERNION (0x01)
-
- /**************************************************************************/
- /* High Rate Proceses */
- /**************************************************************************/
-
-#define MAX_HIGH_RATE_PROCESSES 16
-
- /**************************************************************************/
- /* Prototypes */
- /**************************************************************************/
-
- inv_error_t inv_set_fifo_rate(unsigned short fifoRate);
- unsigned short inv_get_fifo_rate(void);
- int_fast16_t inv_get_sample_step_size_ms(void);
- int_fast16_t inv_get_sample_frequency(void);
- long inv_decode_temperature(short tempReg);
-
- // Register callbacks after a packet of FIFO data is processed
- inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority);
- inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func);
- inv_error_t inv_run_fifo_rate_processes(void);
-
- // Setup FIFO for various output
- inv_error_t inv_send_quaternion(uint_fast16_t accuracy);
- inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy);
- inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy);
- inv_error_t inv_send_linear_accel(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_cntrl_data(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_sensor_data(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_external_sensor_data(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_gravity(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_packet_number(uint_fast16_t accuracy);
- inv_error_t inv_send_quantized_accel(uint_fast16_t elements,
- uint_fast16_t accuracy);
- inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy);
-
- // Get Fixed Point data from FIFO
- inv_error_t inv_get_accel(long *data);
- inv_error_t inv_get_quaternion(long *data);
- inv_error_t inv_get_6axis_quaternion(long *data);
- inv_error_t inv_get_relative_quaternion(long *data);
- inv_error_t inv_get_gyro(long *data);
- inv_error_t inv_set_linear_accel_filter_coef(float coef);
- inv_error_t inv_get_linear_accel(long *data);
- inv_error_t inv_get_linear_accel_in_world(long *data);
- inv_error_t inv_get_gyro_and_accel_sensor(long *data);
- inv_error_t inv_get_gyro_sensor(long *data);
- inv_error_t inv_get_cntrl_data(long *data);
- inv_error_t inv_get_temperature(long *data);
- inv_error_t inv_get_gravity(long *data);
- inv_error_t inv_get_unquantized_accel(long *data);
- inv_error_t inv_get_quantized_accel(long *data);
- inv_error_t inv_get_external_sensor_data(long *data, int size);
- inv_error_t inv_get_eis(long *data);
-
- // Get Floating Point data from FIFO
- inv_error_t inv_get_accel_float(float *data);
- inv_error_t inv_get_quaternion_float(float *data);
-
- inv_error_t inv_process_fifo_packet(const unsigned char *dmpData);
- inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets,
- int_fast8_t * processed);
-
- inv_error_t inv_set_fifo_processed_callback(void (*func) (void));
-
- inv_error_t inv_init_fifo_param(void);
- inv_error_t inv_close_fifo(void);
- inv_error_t inv_set_gyro_data_source(uint_fast8_t source);
- inv_error_t inv_decode_quantized_accel(void);
- unsigned long inv_get_gyro_sum_of_sqr(void);
- unsigned long inv_accel_sum_of_sqr(void);
- void inv_override_quaternion(float *q);
-#ifdef UMPL
- bool isUmplDataInFIFO(void);
- void setUmplDataInFIFOFlag(bool flag);
-#endif
- uint_fast16_t inv_get_fifo_packet_size(void);
-#ifdef __cplusplus
-}
-#endif
-#endif // INVENSENSE_INV_FIFO_H__
diff --git a/60xx/mlsdk/mllite/mlFIFOHW.c b/60xx/mlsdk/mllite/mlFIFOHW.c
deleted file mode 100644
index 7a77e66..0000000
--- a/60xx/mlsdk/mllite/mlFIFOHW.c
+++ /dev/null
@@ -1,328 +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: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *******************************************************************************/
-
-/**
- * @defgroup MLFIFO_HW
- * @brief Motion Library - FIFO HW Driver.
- * Provides facilities to interact with the FIFO.
- *
- * @{
- * @file mlFIFOHW.c
- * @brief The Motion Library Fifo Hardware Layer.
- *
- */
-
-#include <string.h>
-
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-# include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-# include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-# include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-#include "mlFIFOHW.h"
-#include "ml.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-
-#include "mlsl.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-fifo"
-
-/*
- Defines
-*/
-
-#define _fifoDebug(x) //{x}
-
-/*
- Typedefs
-*/
-
-struct fifo_hw_obj {
- short fifoCount;
- inv_error_t fifoError;
- unsigned char fifoOverflow;
- unsigned char fifoResetOnOverflow;
-};
-
-/*
- Global variables
-*/
-const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A };
-
-/*
- Static variables
-*/
-static struct fifo_hw_obj fifo_objHW;
-
-/*
- Definitions
-*/
-
-/**
- * @brief Initializes the internal FIFO data structure.
- */
-void inv_init_fifo_hardare(void)
-{
- memset(&fifo_objHW, 0, sizeof(fifo_objHW));
- fifo_objHW.fifoResetOnOverflow = TRUE;
-}
-
-/**
- * @internal
- * @brief used to get the FIFO data.
- * @param length
- * Number of bytes to read from the FIFO.
- * @param buffer
- * the bytes of FIFO data.
- * Note that this buffer <b>must</b> be large enough
- * to store and additional trailing FIFO footer when
- * expected. The callers must make sure enough space
- * is allocated.
- * @return number of valid bytes of data.
-**/
-uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- uint_fast16_t inFifo;
- uint_fast16_t toRead;
- int_fast8_t kk;
-
- toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
- /*---- make sure length is correct ----*/
- if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
- fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
- return 0;
- }
-
- result = inv_get_fifo_length(&inFifo);
- if (INV_SUCCESS != result) {
- fifo_objHW.fifoError = result;
- return 0;
- }
- // fifo_objHW.fifoCount is the footer size left in the buffer, or
- // 0 if this is the first time reading the fifo since it was reset
- if (inFifo < length + fifo_objHW.fifoCount) {
- fifo_objHW.fifoError = INV_SUCCESS;
- return 0;
- }
- // if a trailing fifo count is expected - start storing data 2 bytes before
- result =
- inv_read_fifo(fifo_objHW.fifoCount >
- 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
- if (INV_SUCCESS != result) {
- fifo_objHW.fifoError = result;
- return 0;
- }
- // Make sure the fifo didn't overflow before or during the read
- result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
- MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
- if (INV_SUCCESS != result) {
- fifo_objHW.fifoError = result;
- return 0;
- }
-
- if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
- MPL_LOGV("Resetting Fifo : Overflow\n");
- inv_reset_fifo();
- fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
- return 0;
- }
-
- /* Check the Footer value to give us a chance at making sure data
- * didn't get corrupted */
- for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
- if (buffer[kk] != gFifoFooter[kk]) {
- MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
- buffer[0], buffer[1]);
- _fifoDebug(char out[200];
- MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
- sprintf(out, "0x");
- for (kk = 0; kk < (int)toRead; kk++) {
- sprintf(out, "%s%02X", out, buffer[kk]);}
- MPL_LOGW("%s\n", out);)
- inv_reset_fifo();
- fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
- return 0;
- }
- }
-
- if (fifo_objHW.fifoCount == 0) {
- fifo_objHW.fifoCount = FIFO_FOOTER_SIZE;
- }
-
- return length - FIFO_FOOTER_SIZE;
-}
-
-/**
- * @brief Used to query the status of the FIFO.
- * @return INV_SUCCESS if the fifo is OK. An error code otherwise.
-**/
-inv_error_t inv_get_fifo_status(void)
-{
- inv_error_t fifoError = fifo_objHW.fifoError;
- fifo_objHW.fifoError = 0;
- return fifoError;
-}
-
-/**
- * @internal
- * @brief Get the length from the fifo
- *
- * @param[out] len amount of data currently stored in the fifo.
- *
- * @return INV_SUCCESS or non-zero error code.
-**/
-inv_error_t inv_get_fifo_length(uint_fast16_t * len)
-{
- INVENSENSE_FUNC_START;
- unsigned char fifoBuf[2];
- inv_error_t result;
-
- if (NULL == len)
- return INV_ERROR_INVALID_PARAMETER;
-
- /*---- read the 2 'count' registers and
- burst read the data from the FIFO ----*/
- result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
- MPUREG_FIFO_COUNTH, 2, fifoBuf);
- if (INV_SUCCESS != result) {
- MPL_LOGE("ReadBurst failed %d\n", result);
- inv_reset_fifo();
- fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT;
- *len = 0;
- return result;
- }
-
- *len = (uint_fast16_t) (fifoBuf[0] << 8);
- *len += (uint_fast16_t) (fifoBuf[1]);
- return result;
-}
-
-/**
- * @brief inv_get_fifo_count is used to get the number of bytes left in the FIFO.
- * This function returns the stored value and does not access the hardware.
- * See inv_get_fifo_length().
- * @return the number of bytes left in the FIFO
-**/
-short inv_get_fifo_count(void)
-{
- return fifo_objHW.fifoCount;
-}
-
-/**
- * @internal
- * @brief Read data from the fifo
- *
- * @param[out] data Location to store the date read from the fifo
- * @param[in] len Amount of data to read out of the fifo
- *
- * @return INV_SUCCESS or non-zero error code
-**/
-inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- result = inv_serial_read_fifo(inv_get_serial_handle(),
- inv_get_mpu_slave_addr(),
- (unsigned short)len, data);
- if (INV_SUCCESS != result) {
- MPL_LOGE("inv_serial_readBurst failed %d\n", result);
- inv_reset_fifo();
- fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA;
- return result;
- }
- return result;
-}
-
-/**
- * @brief Clears the FIFO status and its content.
- * @note Halt the DMP writing into the FIFO for the time
- * needed to reset the FIFO.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_reset_fifo(void)
-{
- INVENSENSE_FUNC_START;
- int len = FIFO_HW_SIZE;
- unsigned char fifoBuf[2];
- unsigned char tries = 0;
- unsigned char userCtrlReg;
- inv_error_t result;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- fifo_objHW.fifoCount = 0;
- if (mldl_cfg->gyro_is_suspended)
- return INV_SUCCESS;
-
- result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
- MPUREG_USER_CTRL, 1, &userCtrlReg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- while (len != 0 && tries < 6) {
- result =
- inv_serial_single_write(inv_get_serial_handle(),
- inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
- ((userCtrlReg & (~BIT_FIFO_EN)) |
- BIT_FIFO_RST));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result =
- inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
- MPUREG_FIFO_COUNTH, 2, fifoBuf);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1];
- tries++;
- }
-
- result =
- inv_serial_single_write(inv_get_serial_handle(),
- inv_get_mpu_slave_addr(), MPUREG_USER_CTRL,
- userCtrlReg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @}
-**/
diff --git a/60xx/mlsdk/mllite/mlFIFOHW.h b/60xx/mlsdk/mllite/mlFIFOHW.h
deleted file mode 100644
index 6f70185..0000000
--- a/60xx/mlsdk/mllite/mlFIFOHW.h
+++ /dev/null
@@ -1,48 +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.
- $
- */
-#ifndef INVENSENSE_INV_FIFO_HW_H__
-#define INVENSENSE_INV_FIFO_HW_H__
-
-#include "mpu.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlFIFOHW_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- // This is the maximum amount of FIFO data we would read in one packet
-#define MAX_FIFO_LENGTH (256)
- // This is the hardware size of the FIFO
-#define FIFO_FOOTER_SIZE (2)
-
- uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer);
- inv_error_t inv_get_fifo_status(void);
- inv_error_t inv_get_fifo_length(uint_fast16_t * len);
- short inv_get_fifo_count(void);
- inv_error_t inv_reset_fifo(void);
- void inv_init_fifo_hardare();
- inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INVENSENSE_INV_FIFO_HW_H__
diff --git a/60xx/mlsdk/mllite/mlMathFunc.c b/60xx/mlsdk/mllite/mlMathFunc.c
deleted file mode 100644
index 0d8e7ec..0000000
--- a/60xx/mlsdk/mllite/mlMathFunc.c
+++ /dev/null
@@ -1,377 +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.
- $
- */
-#include "mlmath.h"
-#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
- * assemblers don't handle well
- * @param[in] a
- * @param[in] b
- * @return ((long long)a*b)>>29
-*/
-long inv_q29_mult(long a, long b)
-{
- long long temp;
- long result;
- temp = (long long)a *b;
- result = (long)(temp >> 29);
- return result;
-}
-
-/** Performs a multiply and shift by 30. These are good functions to write in assembly on
- * with devices with small memory where you want to get rid of the long long which some
- * assemblers don't handle well
- * @param[in] a
- * @param[in] b
- * @return ((long long)a*b)>>30
-*/
-long inv_q30_mult(long a, long b)
-{
- long long temp;
- long result;
- temp = (long long)a *b;
- result = (long)(temp >> 30);
- return result;
-}
-
-void inv_q_mult(const long *q1, const long *q2, long *qProd)
-{
- INVENSENSE_FUNC_START;
- qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] -
- (long long)q1[2] * q2[2] -
- (long long)q1[3] * q2[3]) >> 30);
- qProd[1] =
- (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] +
- (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30);
- qProd[2] =
- (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] +
- (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30);
- qProd[3] =
- (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] -
- (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;
- qInverted[0] = q[0];
- qInverted[1] = -q[1];
- qInverted[2] = -q[2];
- 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.
- * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
- * First 3 elements of the rotation matrix, represent
- * the first row of the matrix. Rotation matrix multiplied
- * by a 3 element column vector transform a vector from Body
- * to World.
- */
-void inv_quaternion_to_rotation(const long *quat, long *rot)
-{
- rot[0] =
- inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
- quat[0]) - 1073741824L;
- rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
- rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
- rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
- rot[4] =
- inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
- quat[0]) - 1073741824L;
- rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
- rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
- rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
- rot[8] =
- inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
- quat[0]) - 1073741824L;
-}
-
-/** Converts a 32-bit long to a big endian byte stream */
-unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
-{
- big8[0] = (unsigned char)((x >> 24) & 0xff);
- big8[1] = (unsigned char)((x >> 16) & 0xff);
- big8[2] = (unsigned char)((x >> 8) & 0xff);
- big8[3] = (unsigned char)(x & 0xff);
- return big8;
-}
-
-/** Converts a big endian byte stream into a 32-bit long */
-long inv_big8_to_int32(const unsigned char *big8)
-{
- long x;
- x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) |
- ((long)big8[3]);
- return x;
-}
-
-/** Converts a 16-bit short to a big endian byte stream */
-unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
-{
- big8[0] = (unsigned char)((x >> 8) & 0xff);
- big8[1] = (unsigned char)(x & 0xff);
- return big8;
-}
-
-void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
-{
- int k, l, i, j;
- for (i = 0, k = 0; i < *n; i++, k++) {
- for (j = 0, l = 0; j < *n; j++, l++) {
- if (i == x)
- i++;
- if (j == y)
- j++;
- *(b + 10 * k + l) = *(a + 10 * i + j);
- }
- }
- *n = *n - 1;
-}
-
-void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
-{
- int k, l, i, j;
- for (i = 0, k = 0; i < *n; i++, k++) {
- for (j = 0, l = 0; j < *n; j++, l++) {
- if (i == x)
- i++;
- if (j == y)
- j++;
- *(b + 10 * k + l) = *(a + 10 * i + j);
- }
- }
- *n = *n - 1;
-}
-
-float inv_matrix_det(float *p, int *n)
-{
- float 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_inc(p, &d[0][0], n, i, j);
- sum =
- sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0],
- 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]
- */
-float inv_wrap_angle(float ang)
-{
- if (ang > M_PI)
- return ang - 2 * (float)M_PI;
- else if (ang <= -(float)M_PI)
- return ang + 2 * (float)M_PI;
- else
- return ang;
-}
-
-/** Finds the minimum angle difference ang1-ang2 such that difference
- * is between [-M_PI,M_PI]
- * @param[in] ang1
- * @param[in] ang2
- * @return angle difference ang1-ang2
- */
-float inv_angle_diff(float ang1, float ang2)
-{
- float d;
- ang1 = inv_wrap_angle(ang1);
- ang2 = inv_wrap_angle(ang2);
- d = ang1 - ang2;
- if (d > M_PI)
- d -= 2 * (float)M_PI;
- else if (d < -(float)M_PI)
- d += 2 * (float)M_PI;
- return d;
-}
diff --git a/60xx/mlsdk/mllite/mlMathFunc.h b/60xx/mlsdk/mllite/mlMathFunc.h
deleted file mode 100644
index 70fa9f4..0000000
--- a/60xx/mlsdk/mllite/mlMathFunc.h
+++ /dev/null
@@ -1,68 +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.
- $
- */
-#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);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INVENSENSE_INV_MATH_FUNC_H__
diff --git a/60xx/mlsdk/mllite/mlSetGyroBias.c b/60xx/mlsdk/mllite/mlSetGyroBias.c
deleted file mode 100644
index bd14d2e..0000000
--- a/60xx/mlsdk/mllite/mlSetGyroBias.c
+++ /dev/null
@@ -1,198 +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:$
- *
- *****************************************************************************/
-
-#include "mlSetGyroBias.h"
-#include "mlFIFO.h"
-#include "ml.h"
-#include <string.h>
-#include "mldl.h"
-#include "mlMathFunc.h"
-
-typedef struct {
- int needToSetBias;
- short currentBias[3];
- int mode;
- int motion;
-} tSGB;
-
-tSGB sgb;
-
-/** Records a motion event that may cause a callback when the priority for this
- * feature is met.
- */
-void inv_set_motion_state(int motion)
-{
- sgb.motion = motion;
-}
-
-/** Converts from internal DMP gyro bias registers to external hardware gyro bias by
-* applying scaling and transformation.
-*/
-void inv_convert_bias(const unsigned char *regs, short *bias)
-{
- long biasTmp2[3], biasTmp[3], biasPrev[3];
- int i;
- int sf;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (mldl_cfg->gyro_sens_trim != 0) {
- sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
- } else {
- sf = 2000;
- }
- for (i = 0; i < 3; i++) {
- biasTmp2[i] = inv_big8_to_int32(®s[i * 4]);
- }
- // Rotate bias vector by the transpose of the orientation matrix
- for (i = 0; i < 3; ++i) {
- biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.gyro_orient[i]) +
- inv_q30_mult(biasTmp2[1], inv_obj.gyro_orient[i + 3]) +
- inv_q30_mult(biasTmp2[2], inv_obj.gyro_orient[i + 6]);
- }
-
- for (i = 0; i < GYRO_NUM_AXES; i++) {
- biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf);
- biasPrev[i] = (long)mldl_cfg->offset[i];
- if (biasPrev[i] > 32767)
- biasPrev[i] -= 65536L;
- }
-
- for (i = 0; i < GYRO_NUM_AXES; i++) {
- bias[i] = (short)(biasPrev[i] - biasTmp[i]);
- }
-}
-
-/** Records hardware biases in format as used by hardware gyro registers.
-* Note, the hardware will add this value to the measured gyro data.
-*/
-inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode)
-{
- if (sgb.currentBias[0] != bias[0])
- sgb.needToSetBias = 1;
- if (sgb.currentBias[1] != bias[1])
- sgb.needToSetBias = 1;
- if (sgb.currentBias[2] != bias[2])
- sgb.needToSetBias = 1;
- if (sgb.needToSetBias) {
- memcpy(sgb.currentBias, bias, sizeof(sgb.currentBias));
- sgb.mode = mode;
- }
- return INV_SUCCESS;
-}
-
-/** Records gyro biases
-* @param[in] bias Bias where 1dps is 2^16. In chip frame.
-*/
-inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode)
-{
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- int sf, i;
- long biasTmp;
- short offset[3];
- inv_error_t result;
-
- if (mldl_cfg->gyro_sens_trim != 0) {
- sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
- } else {
- sf = 2000;
- }
-
- for (i = 0; i < GYRO_NUM_AXES; i++) {
- biasTmp = -bias[i] / sf;
- if (biasTmp < 0)
- biasTmp += 65536L;
- offset[i] = (short)biasTmp;
- }
- result = inv_set_gyro_bias_in_hw_unit(offset, mode);
- return result;
-}
-
-inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode)
-{
- long biasL[3];
- inv_error_t result;
-
- biasL[0] = (long)(bias[0] * (1L << 16));
- biasL[1] = (long)(bias[1] * (1L << 16));
- biasL[2] = (long)(bias[2] * (1L << 16));
- result = inv_set_gyro_bias_in_dps(biasL, mode);
- return result;
-}
-
-inv_error_t MLSetGyroBiasCB(struct inv_obj_t * inv_obj)
-{
- inv_error_t result = INV_SUCCESS;
- if (sgb.needToSetBias) {
- result = inv_set_offset(sgb.currentBias);
- sgb.needToSetBias = 0;
- }
-
- // Check if motion state has changed
- if (sgb.motion == INV_MOTION) {
- // We are moving
- if (inv_obj->motion_state == INV_NO_MOTION) {
- //Trigger motion callback
- inv_obj->motion_state = INV_MOTION;
- inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
- if (inv_params_obj.motion_cb_func) {
- inv_params_obj.motion_cb_func(INV_MOTION);
- }
- }
- } else if (sgb.motion == INV_NO_MOTION){
- // We are not moving
- if (inv_obj->motion_state == INV_MOTION) {
- //Trigger no motion callback
- inv_obj->motion_state = INV_NO_MOTION;
- inv_obj->got_no_motion_bias = TRUE;
- inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_NO_MOTION;
- if (inv_params_obj.motion_cb_func) {
- inv_params_obj.motion_cb_func(INV_NO_MOTION);
- }
- }
- }
-
- return result;
-}
-
-inv_error_t inv_enable_set_bias(void)
-{
- inv_error_t result;
- memset(&sgb, 0, sizeof(sgb));
-
- sgb.motion = inv_obj.motion_state;
-
- result =
- inv_register_fifo_rate_process(MLSetGyroBiasCB,
- INV_PRIORITY_SET_GYRO_BIASES);
- if (result == INV_ERROR_INVALID_PARAMETER)
- result = INV_SUCCESS; /* We already registered this */
- return result;
-}
-
-inv_error_t inv_disable_set_bias(void)
-{
- inv_error_t result;
- result = inv_unregister_fifo_rate_process(MLSetGyroBiasCB);
- return INV_SUCCESS; // FIXME need to disable
-}
diff --git a/60xx/mlsdk/mllite/mlSetGyroBias.h b/60xx/mlsdk/mllite/mlSetGyroBias.h
deleted file mode 100644
index e56f18b..0000000
--- a/60xx/mlsdk/mllite/mlSetGyroBias.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$
- *
- *****************************************************************************/
-
-#ifndef INV_SET_GYRO_BIAS__H__
-#define INV_SET_GYRO_BIAS__H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define INV_SGB_NO_MOTION 4
-#define INV_SGB_FAST_NO_MOTION 5
-#define INV_SGB_TEMP_COMP 6
-
- inv_error_t inv_enable_set_bias(void);
- inv_error_t inv_disable_set_bias(void);
- inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode);
- inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode);
- inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode);
- void inv_convert_bias(const unsigned char *regs, short *bias);
- void inv_set_motion_state(int motion);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INV_SET_GYRO_BIAS__H__
diff --git a/60xx/mlsdk/mllite/ml_mputest.c b/60xx/mlsdk/mllite/ml_mputest.c
deleted file mode 100644
index d7fc608..0000000
--- a/60xx/mlsdk/mllite/ml_mputest.c
+++ /dev/null
@@ -1,184 +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)
-{
-#ifdef CONFIG_MPU_SENSORS_MPU3050
- return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE);
-#else
- return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
-#endif
-}
-
-/**
- * @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/60xx/mlsdk/mllite/ml_mputest.h b/60xx/mlsdk/mllite/ml_mputest.h
deleted file mode 100644
index 705d3cc..0000000
--- a/60xx/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/60xx/mlsdk/mllite/ml_stored_data.c b/60xx/mlsdk/mllite/ml_stored_data.c
deleted file mode 100644
index 9107fe2..0000000
--- a/60xx/mlsdk/mllite/ml_stored_data.c
+++ /dev/null
@@ -1,1586 +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_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML_STORED_DATA
- *
- * @{
- * @file ml_stored_data.c
- * @brief functions for reading and writing stored data sets.
- * Typically, these functions process stored calibration data.
- */
-
-#include "ml_stored_data.h"
-#include "ml.h"
-#include "mlFIFO.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "compass.h"
-#include "dmpKey.h"
-#include "dmpDefault.h"
-#include "mlstates.h"
-#include "checksum.h"
-#include "mlsupervisor.h"
-
-#include "mlsl.h"
-#include "mlos.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-storeload"
-
-/*
- Typedefs
-*/
-typedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short);
-
-/*
- Globals
-*/
-extern struct inv_obj_t inv_obj;
-
-/*
- Debugging Definitions
- set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields
- being read or stored in human-readable format.
- When set to 0, the compiler will optimize and remove the printouts
- from the code.
-*/
-#define LOADCAL_DEBUG 0
-#define STORECAL_DEBUG 0
-
-#define LOADCAL_LOG(...) \
- if(LOADCAL_DEBUG) \
- MPL_LOGI("LOADCAL: " __VA_ARGS__)
-#define STORECAL_LOG(...) \
- if(STORECAL_DEBUG) \
- MPL_LOGI("STORECAL: " __VA_ARGS__)
-
-/**
- * @brief Duplicate of the inv_temp_comp_find_bin function in the libmpl
- * advanced algorithms library. To remove cross-dependency, for now,
- * we reimplement the same function here.
- * @param temp
- * the temperature (1 count == 1 degree C).
- */
-int FindTempBin(float temp)
-{
- int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN);
- if (bin < 0)
- bin = 0;
- if (bin > BINS - 1)
- bin = BINS - 1;
- return bin;
-}
-
-/**
- * @brief Returns the length of the <b>MPL internal calibration data</b>.
- * Should be called before allocating the memory required to store
- * this data to a file.
- * This function returns the total size required to store the cal
- * data including the header (4 bytes) and the checksum (2 bytes).
- *
- * @pre Must be in INV_STATE_DMP_OPENED state.
- * inv_dmp_open() or inv_dmp_stop() must have been called.
- * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- * been called.
- *
- * @return the length of the internal calibrated data format.
- */
-unsigned int inv_get_cal_length(void)
-{
- INVENSENSE_FUNC_START;
- unsigned int length;
- length = INV_CAL_HDR_LEN + // header
- BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal
- INV_CAL_ACCEL_LEN + // accel cal
- INV_CAL_COMPASS_LEN + // compass cal
- INV_CAL_CHK_LEN; // checksum
- return length;
-}
-
-/**
- * @brief Loads a type 0 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature,
- * - gyro biases for X, Y, Z axes.
- * This calibration data would normally be produced by the MPU Self
- * Test and its size is 18 bytes (header and checksum included).
- * Calibration format type 0 is currently <b>NOT</b> used and
- * is substituted by type 5 : inv_load_cal_V5().
- *
- * @note This calibration data format is obsoleted and no longer supported
- * by the rest of the MPL
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len)
-{
- INVENSENSE_FUNC_START;
- const short expLen = 18;
- long newGyroData[3] = { 0, 0, 0 };
- float newTemp;
- int bin;
-
- LOADCAL_LOG("Entering inv_load_cal_V0\n");
-
- if (len != expLen) {
- MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen);
- return INV_ERROR_FILE_READ;
- }
-
- newTemp = (float)inv_decode_temperature(
- (unsigned short)calData[6] * 256 + calData[7]) / 65536.f;
- LOADCAL_LOG("newTemp = %f\n", newTemp);
-
- newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]);
- if (newGyroData[0] > 32767L)
- newGyroData[0] -= 65536L;
- LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]);
- newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]);
- if (newGyroData[1] > 32767L)
- newGyroData[1] -= 65536L;
- LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]);
- newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]);
- if (newGyroData[2] > 32767L)
- newGyroData[2] -= 65536L;
- LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]);
-
- bin = FindTempBin(newTemp);
- LOADCAL_LOG("bin = %d", bin);
-
- inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp;
- LOADCAL_LOG("temp_data[%d][%d] = %f",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]);
- inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] =
- ((float)newGyroData[0]) / 65536.f;
- LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
- inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] =
- ((float)newGyroData[0]) / 65536.f;
- LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
- inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] =
- ((float)newGyroData[0]) / 65536.f;
- LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-
- inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN;
- LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]);
-
- if (inv_obj.temp_ptrs[bin] == 0)
- inv_obj.temp_valid_data[bin] = TRUE;
- LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
- bin, inv_obj.temp_valid_data[bin]);
-
- inv_obj.got_no_motion_bias = TRUE;
- LOADCAL_LOG("got_no_motion_bias = 1\n");
- inv_obj.cal_loaded_flag = TRUE;
- LOADCAL_LOG("cal_loaded_flag = 1\n");
-
- LOADCAL_LOG("Exiting inv_load_cal_V0\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a type 1 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature,
- * - gyro biases for X, Y, Z axes,
- * - accel biases for X, Y, Z axes.
- * This calibration data would normally be produced by the MPU Self
- * Test and its size is 24 bytes (header and checksum included).
- * Calibration format type 1 is currently <b>NOT</b> used and
- * is substituted by type 5 : inv_load_cal_V5().
- *
- * @note In order to successfully work, the gyro bias must be stored
- * expressed in 250 dps full scale (131.072 sensitivity). Other full
- * scale range will produce unpredictable results in the gyro biases.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len)
-{
- INVENSENSE_FUNC_START;
- const short expLen = 24;
- long newGyroData[3] = {0, 0, 0};
- long accelBias[3] = {0, 0, 0};
- float gyroBias[3] = {0, 0, 0};
- float newTemp = 0;
- int bin;
-
- LOADCAL_LOG("Entering inv_load_cal_V1\n");
-
- if (len != expLen) {
- MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen);
- return INV_ERROR_FILE_READ;
- }
-
- newTemp = (float)inv_decode_temperature(
- (unsigned short)calData[6] * 256 + calData[7]) / 65536.f;
- LOADCAL_LOG("newTemp = %f\n", newTemp);
-
- newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]);
- if (newGyroData[0] > 32767L)
- newGyroData[0] -= 65536L;
- LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]);
- newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]);
- if (newGyroData[1] > 32767L)
- newGyroData[1] -= 65536L;
- LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]);
- newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]);
- if (newGyroData[2] > 32767L)
- newGyroData[2] -= 65536L;
- LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]);
-
- bin = FindTempBin(newTemp);
- LOADCAL_LOG("bin = %d\n", bin);
-
- gyroBias[0] = ((float)newGyroData[0]) / 131.072f;
- gyroBias[1] = ((float)newGyroData[1]) / 131.072f;
- gyroBias[2] = ((float)newGyroData[2]) / 131.072f;
- LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]);
- LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]);
- LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]);
-
- inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp;
- LOADCAL_LOG("temp_data[%d][%d] = %f",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]);
- inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0];
- LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
- inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1];
- LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
- inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2];
- LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin],
- inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]);
-
- inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN;
- LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]);
-
- if (inv_obj.temp_ptrs[bin] == 0)
- inv_obj.temp_valid_data[bin] = TRUE;
- LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
- bin, inv_obj.temp_valid_data[bin]);
-
- /* load accel biases and apply immediately */
- accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]);
- if (accelBias[0] > 32767)
- accelBias[0] -= 65536L;
- accelBias[0] = (long)((long long)accelBias[0] * 65536L *
- inv_obj.accel_sens / 1073741824L);
- LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]);
- accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]);
- if (accelBias[1] > 32767)
- accelBias[1] -= 65536L;
- accelBias[1] = (long)((long long)accelBias[1] * 65536L *
- inv_obj.accel_sens / 1073741824L);
- LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]);
- accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]);
- if (accelBias[2] > 32767)
- accelBias[2] -= 65536L;
- 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);
- }
-
- inv_obj.got_no_motion_bias = TRUE;
- LOADCAL_LOG("got_no_motion_bias = 1\n");
- inv_obj.cal_loaded_flag = TRUE;
- LOADCAL_LOG("cal_loaded_flag = 1\n");
-
- LOADCAL_LOG("Exiting inv_load_cal_V1\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a type 2 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature compensation : temperature data points,
- * - temperature compensation : gyro biases data points for X, Y,
- * and Z axes.
- * - accel biases for X, Y, Z axes.
- * This calibration data is produced internally by the MPL and its
- * size is 2222 bytes (header and checksum included).
- * Calibration format type 2 is currently <b>NOT</b> used and
- * is substituted by type 4 : inv_load_cal_V4().
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len)
-{
- INVENSENSE_FUNC_START;
- const short expLen = 2222;
- long accel_bias[3];
- int ptr = INV_CAL_HDR_LEN;
-
- int i, j;
- long long tmp;
-
- LOADCAL_LOG("Entering inv_load_cal_V2\n");
-
- if (len != expLen) {
- MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n",
- expLen, len);
- return INV_ERROR_FILE_READ;
- }
-
- for (i = 0; i < BINS; i++) {
- inv_obj.temp_ptrs[i] = 0;
- inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
- inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
- inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
- inv_obj.temp_ptrs[i] += (int)calData[ptr++];
- LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]);
- }
- for (i = 0; i < BINS; i++) {
- inv_obj.temp_valid_data[i] = 0;
- inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
- inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
- inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
- inv_obj.temp_valid_data[i] += (int)calData[ptr++];
- LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
- i, inv_obj.temp_valid_data[i]);
- }
-
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("temp_data[%d][%d] = %f\n",
- i, j, inv_obj.temp_data[i][j]);
- }
-
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.x_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.y_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.z_gyro_temp_data[i][j]);
- }
- }
-
- /* read the accel biases */
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- accel_bias[i] = (int32_t) t;
- 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);
- }
-
- inv_obj.got_no_motion_bias = TRUE;
- LOADCAL_LOG("got_no_motion_bias = 1\n");
- inv_obj.cal_loaded_flag = TRUE;
- LOADCAL_LOG("cal_loaded_flag = 1\n");
-
- LOADCAL_LOG("Exiting inv_load_cal_V2\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a type 3 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature compensation : temperature data points,
- * - temperature compensation : gyro biases data points for X, Y,
- * and Z axes.
- * - accel biases for X, Y, Z axes.
- * - compass biases for X, Y, Z axes and bias tracking algorithm
- * mock-up.
- * This calibration data is produced internally by the MPL and its
- * size is 2429 bytes (header and checksum included).
- * Calibration format type 3 is currently <b>NOT</b> used and
- * is substituted by type 4 : inv_load_cal_V4().
-
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len)
-{
- INVENSENSE_FUNC_START;
- union doubleToLongLong {
- double db;
- unsigned long long ll;
- } dToLL;
-
- const short expLen = 2429;
- long bias[3];
- int i, j;
- int ptr = INV_CAL_HDR_LEN;
- long long tmp;
-
- LOADCAL_LOG("Entering inv_load_cal_V3\n");
-
- if (len != expLen) {
- MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n",
- expLen, len);
- return INV_ERROR_FILE_READ;
- }
-
- for (i = 0; i < BINS; i++) {
- inv_obj.temp_ptrs[i] = 0;
- inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
- inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
- inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
- inv_obj.temp_ptrs[i] += (int)calData[ptr++];
- LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]);
- }
- for (i = 0; i < BINS; i++) {
- inv_obj.temp_valid_data[i] = 0;
- inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
- inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
- inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
- inv_obj.temp_valid_data[i] += (int)calData[ptr++];
- LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
- i, inv_obj.temp_valid_data[i]);
- }
-
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("temp_data[%d][%d] = %f\n",
- i, j, inv_obj.temp_data[i][j]);
- }
- }
-
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.x_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.y_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.z_gyro_temp_data[i][j]);
- }
- }
-
- /* read the accel biases */
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- 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);
- }
-
- /* read the compass biases */
- inv_obj.got_compass_bias = (int)calData[ptr++];
- inv_obj.got_init_compass_bias = (int)calData[ptr++];
- inv_obj.compass_state = (int)calData[ptr++];
-
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_bias_error[i] = (int32_t) t;
- LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i,
- inv_obj.compass_bias_error[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.init_compass_bias[i] = (int32_t) t;
- LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i,
- inv_obj.init_compass_bias[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_bias[i] = (int32_t) t;
- LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]);
- }
- for (i = 0; i < 18; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_peaks[i] = (int32_t) t;
- LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]);
- }
- for (i = 0; i < 3; i++) {
- dToLL.ll = 0;
- dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += (unsigned long long)calData[ptr++];
-
- inv_obj.compass_bias_v[i] = dToLL.db;
- LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]);
- }
- for (i = 0; i < 9; i++) {
- dToLL.ll = 0;
- dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += (unsigned long long)calData[ptr++];
-
- inv_obj.compass_bias_ptr[i] = dToLL.db;
- LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i,
- inv_obj.compass_bias_ptr[i]);
- }
-
- inv_obj.got_no_motion_bias = TRUE;
- LOADCAL_LOG("got_no_motion_bias = 1\n");
- inv_obj.cal_loaded_flag = TRUE;
- LOADCAL_LOG("cal_loaded_flag = 1\n");
-
- LOADCAL_LOG("Exiting inv_load_cal_V3\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a type 4 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature compensation : temperature data points,
- * - temperature compensation : gyro biases data points for X, Y,
- * and Z axes.
- * - accel biases for X, Y, Z axes.
- * - compass biases for X, Y, Z axes, compass scale, and bias
- * tracking algorithm mock-up.
- * This calibration data is produced internally by the MPL and its
- * size is 2777 bytes (header and checksum included).
- * Calibration format type 4 is currently used and
- * substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()).
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len)
-{
- INVENSENSE_FUNC_START;
- union doubleToLongLong {
- double db;
- unsigned long long ll;
- } dToLL;
-
- const unsigned int expLen = 2782;
- long bias[3];
- int ptr = INV_CAL_HDR_LEN;
- int i, j;
- long long tmp;
- inv_error_t result;
-
- LOADCAL_LOG("Entering inv_load_cal_V4\n");
-
- if (len != expLen) {
- MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n",
- expLen, len);
- return INV_ERROR_FILE_READ;
- }
-
- for (i = 0; i < BINS; i++) {
- inv_obj.temp_ptrs[i] = 0;
- inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
- inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
- inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
- inv_obj.temp_ptrs[i] += (int)calData[ptr++];
- LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]);
- }
- for (i = 0; i < BINS; i++) {
- inv_obj.temp_valid_data[i] = 0;
- inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
- inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
- inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
- inv_obj.temp_valid_data[i] += (int)calData[ptr++];
- LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
- i, inv_obj.temp_valid_data[i]);
- }
-
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("temp_data[%d][%d] = %f\n",
- i, j, inv_obj.temp_data[i][j]);
- }
- }
-
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.x_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.y_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = 0;
- tmp += 16777216LL * (long long)calData[ptr++];
- tmp += 65536LL * (long long)calData[ptr++];
- tmp += 256LL * (long long)calData[ptr++];
- tmp += (long long)calData[ptr++];
- if (tmp > 2147483648LL) {
- tmp -= 4294967296LL;
- }
- inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f;
- LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.z_gyro_temp_data[i][j]);
- }
- }
-
- /* read the accel biases */
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- 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);
- }
-
- /* read the compass biases */
- inv_reset_compass_calibration();
-
- inv_obj.got_compass_bias = (int)calData[ptr++];
- LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias);
- inv_obj.got_init_compass_bias = (int)calData[ptr++];
- LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias);
- inv_obj.compass_state = (int)calData[ptr++];
- LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state);
-
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_bias_error[i] = (int32_t) t;
- LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i,
- inv_obj.compass_bias_error[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.init_compass_bias[i] = (int32_t) t;
- LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i,
- inv_obj.init_compass_bias[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_bias[i] = (int32_t) t;
- LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]);
- }
- for (i = 0; i < 18; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_peaks[i] = (int32_t) t;
- LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]);
- }
- for (i = 0; i < 3; i++) {
- dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += (unsigned long long)calData[ptr++];
-
- inv_obj.compass_bias_v[i] = dToLL.db;
- LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]);
- }
- for (i = 0; i < 9; i++) {
- dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += (unsigned long long)calData[ptr++];
-
- inv_obj.compass_bias_ptr[i] = dToLL.db;
- LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i,
- inv_obj.compass_bias_ptr[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = 0;
- t += 16777216UL * ((uint32_t) calData[ptr++]);
- t += 65536UL * ((uint32_t) calData[ptr++]);
- t += 256u * ((uint32_t) calData[ptr++]);
- t += (uint32_t) calData[ptr++];
- inv_obj.compass_scale[i] = (int32_t) t;
- LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t);
- }
- for (i = 0; i < 6; i++) {
- dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += (unsigned long long)calData[ptr++];
-
- inv_obj.compass_prev_xty[i] = dToLL.db;
- LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db);
- }
- for (i = 0; i < 36; i++) {
- dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
- dToLL.ll += (unsigned long long)calData[ptr++];
-
- inv_obj.compass_prev_m[i] = dToLL.db;
- LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db);
- }
-
- /* Load the compass offset flag and values */
- inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++];
- inv_obj.compass_offsets[0] = calData[ptr++];
- inv_obj.compass_offsets[1] = calData[ptr++];
- inv_obj.compass_offsets[2] = calData[ptr++];
-
- inv_obj.compass_accuracy = calData[ptr++];
- /* push the compass offset values to the device */
- result = inv_set_compass_offset();
-
- if (result == INV_SUCCESS) {
- if (inv_compass_check_range() != INV_SUCCESS) {
- MPL_LOGI("range check fail");
- inv_reset_compass_calibration();
- inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
- inv_set_compass_offset();
- }
- }
-
- inv_obj.got_no_motion_bias = TRUE;
- LOADCAL_LOG("got_no_motion_bias = 1\n");
- inv_obj.cal_loaded_flag = TRUE;
- LOADCAL_LOG("cal_loaded_flag = 1\n");
-
- LOADCAL_LOG("Exiting inv_load_cal_V4\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a type 5 set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- * This calibrations data format stores values for (in order of
- * appearance) :
- * - temperature,
- * - gyro biases for X, Y, Z axes,
- * - accel biases for X, Y, Z axes.
- * This calibration data would normally be produced by the MPU Self
- * Test and its size is 36 bytes (header and checksum included).
- * Calibration format type 5 is produced by the MPU Self Test and
- * substitutes the type 1 : inv_load_cal_V1().
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- * @param len
- * the length of the calibration
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len)
-{
- INVENSENSE_FUNC_START;
- const short expLen = 36;
- long accelBias[3] = { 0, 0, 0 };
- float gyroBias[3] = { 0, 0, 0 };
-
- int ptr = INV_CAL_HDR_LEN;
- unsigned short temp;
- float newTemp;
- int bin;
- int i;
-
- LOADCAL_LOG("Entering inv_load_cal_V5\n");
-
- if (len != expLen) {
- MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n",
- expLen, len);
- return INV_ERROR_FILE_READ;
- }
-
- /* load the temperature */
- temp = (unsigned short)calData[ptr++] * (1L << 8);
- temp += calData[ptr++];
- newTemp = (float)inv_decode_temperature(temp) / 65536.f;
- LOADCAL_LOG("newTemp = %f\n", newTemp);
-
- /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */
- for (i = 0; i < 3; i++) {
- int32_t tmp = 0;
- tmp += (int32_t) calData[ptr++] * (1L << 24);
- tmp += (int32_t) calData[ptr++] * (1L << 16);
- tmp += (int32_t) calData[ptr++] * (1L << 8);
- tmp += (int32_t) calData[ptr++];
- gyroBias[i] = (float)tmp / 65536.0f;
- LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]);
- }
- /* find the temperature bin */
- bin = FindTempBin(newTemp);
-
- /* populate the temp comp data structure */
- inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp;
- LOADCAL_LOG("temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin], newTemp);
-
- inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0];
- LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin], gyroBias[0]);
- inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1];
- LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin], gyroBias[1]);
- inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2];
- LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- bin, inv_obj.temp_ptrs[bin], gyroBias[2]);
- inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN;
- LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]);
-
- if (inv_obj.temp_ptrs[bin] == 0)
- inv_obj.temp_valid_data[bin] = TRUE;
- LOADCAL_LOG("temp_valid_data[%d] = %ld\n",
- bin, inv_obj.temp_valid_data[bin]);
-
- /* load accel biases (represented in 2 ^ 16 == 1 gee)
- and apply immediately */
- for (i = 0; i < 3; i++) {
- int32_t tmp = 0;
- tmp += (int32_t) calData[ptr++] * (1L << 24);
- tmp += (int32_t) calData[ptr++] * (1L << 16);
- tmp += (int32_t) calData[ptr++] * (1L << 8);
- tmp += (int32_t) calData[ptr++];
- 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);
- }
-
- inv_obj.got_no_motion_bias = TRUE;
- LOADCAL_LOG("got_no_motion_bias = 1\n");
- inv_obj.cal_loaded_flag = TRUE;
- LOADCAL_LOG("cal_loaded_flag = 1\n");
-
- LOADCAL_LOG("Exiting inv_load_cal_V5\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Loads a set of calibration data.
- * It parses a binary data set containing calibration data.
- * The binary data set is intended to be loaded from a file.
- *
- * @pre inv_dmp_open()
- * @ifnot MPL_MF
- * or inv_open_low_power_pedometer()
- * or inv_eis_open_dmp()
- * @endif
- * must have been called.
- *
- * @param calData
- * A pointer to an array of bytes to be parsed.
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_cal(unsigned char *calData)
-{
- INVENSENSE_FUNC_START;
- int calType = 0;
- int len = 0;
- int ptr;
- uint32_t chk = 0;
- uint32_t cmp_chk = 0;
-
- tMLLoadFunc loaders[] = {
- inv_load_cal_V0,
- inv_load_cal_V1,
- inv_load_cal_V2,
- inv_load_cal_V3,
- inv_load_cal_V4,
- inv_load_cal_V5
- };
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- /* read the header (type and len)
- len is the total record length including header and checksum */
- len = 0;
- len += 16777216L * ((int)calData[0]);
- len += 65536L * ((int)calData[1]);
- len += 256 * ((int)calData[2]);
- len += (int)calData[3];
-
- calType = ((int)calData[4]) * 256 + ((int)calData[5]);
- if (calType > 5) {
- MPL_LOGE("Unsupported calibration file format %d. "
- "Valid types 0..5\n", calType);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- /* check the checksum */
- chk = 0;
- ptr = len - INV_CAL_CHK_LEN;
-
- chk += 16777216L * ((uint32_t) calData[ptr++]);
- chk += 65536L * ((uint32_t) calData[ptr++]);
- chk += 256 * ((uint32_t) calData[ptr++]);
- chk += (uint32_t) calData[ptr++];
-
- cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN,
- len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN));
-
- if (chk != cmp_chk) {
- return INV_ERROR_CALIBRATION_CHECKSUM;
- }
-
- /* call the proper method to read in the data */
- return loaders[calType] (calData, len);
-}
-
-/**
- * @brief Stores a set of calibration data.
- * It generates a binary data set containing calibration data.
- * The binary data set is intended to be stored into a file.
- *
- * @pre inv_dmp_open()
- *
- * @param calData
- * A pointer to an array of bytes to be stored.
- * @param length
- * The amount of bytes available in the array.
- *
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_store_cal(unsigned char *calData, int length)
-{
- INVENSENSE_FUNC_START;
- int ptr = 0;
- int i = 0;
- int j = 0;
- long long tmp;
- uint32_t chk;
- long bias[3];
- //unsigned char state;
- union doubleToLongLong {
- double db;
- unsigned long long ll;
- } dToLL;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- STORECAL_LOG("Entering inv_store_cal\n");
-
- // length
- calData[0] = (unsigned char)((length >> 24) & 0xff);
- calData[1] = (unsigned char)((length >> 16) & 0xff);
- calData[2] = (unsigned char)((length >> 8) & 0xff);
- calData[3] = (unsigned char)(length & 0xff);
- STORECAL_LOG("calLen = %d\n", length);
-
- // calibration data format type
- calData[4] = 0;
- calData[5] = 4;
- STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]);
-
- // data
- ptr = 6;
- for (i = 0; i < BINS; i++) {
- tmp = (int)inv_obj.temp_ptrs[i];
- calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(tmp & 0xff);
- STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp);
- }
-
- for (i = 0; i < BINS; i++) {
- tmp = (int)inv_obj.temp_valid_data[i];
- calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(tmp & 0xff);
- STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp);
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f);
- if (tmp < 0) {
- tmp += 4294967296LL;
- }
- calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(tmp & 0xff);
- STORECAL_LOG("temp_data[%d][%d] = %f\n",
- i, j, inv_obj.temp_data[i][j]);
- }
- }
-
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f);
- if (tmp < 0) {
- tmp += 4294967296LL;
- }
- calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(tmp & 0xff);
- STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.x_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f);
- if (tmp < 0) {
- tmp += 4294967296LL;
- }
- calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(tmp & 0xff);
- STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.y_gyro_temp_data[i][j]);
- }
- }
- for (i = 0; i < BINS; i++) {
- for (j = 0; j < PTS_PER_BIN; j++) {
- tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f);
- if (tmp < 0) {
- tmp += 4294967296LL;
- }
- calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(tmp & 0xff);
- STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n",
- i, j, inv_obj.z_gyro_temp_data[i][j]);
- }
- }
-
- inv_get_array(INV_ACCEL_BIAS, bias);
-
- /* write the accel biases */
- for (i = 0; i < 3; i++) {
- uint32_t t = (uint32_t) bias[i];
- calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(t & 0xff);
- STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]);
- }
-
- /* write the compass calibration state */
- calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias);
- STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias);
- calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias);
- STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias);
- if (inv_obj.compass_state == SF_UNCALIBRATED) {
- calData[ptr++] = SF_UNCALIBRATED;
- } else {
- calData[ptr++] = SF_STARTUP_SETTLE;
- }
- STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state);
-
- for (i = 0; i < 3; i++) {
- uint32_t t = (uint32_t) inv_obj.compass_bias_error[i];
- calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(t & 0xff);
- STORECAL_LOG("compass_bias_error[%d] = %ld\n",
- i, inv_obj.compass_bias_error[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = (uint32_t) inv_obj.init_compass_bias[i];
- calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(t & 0xff);
- STORECAL_LOG("init_compass_bias[%d] = %ld\n", i,
- inv_obj.init_compass_bias[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = (uint32_t) inv_obj.compass_bias[i];
- calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(t & 0xff);
- STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]);
- }
- for (i = 0; i < 18; i++) {
- uint32_t t = (uint32_t) inv_obj.compass_peaks[i];
- calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(t & 0xff);
- STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]);
- }
- for (i = 0; i < 3; i++) {
- dToLL.db = inv_obj.compass_bias_v[i];
- calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
- STORECAL_LOG("compass_bias_v[%d] = %lf\n", i,
- inv_obj.compass_bias_v[i]);
- }
- for (i = 0; i < 9; i++) {
- dToLL.db = inv_obj.compass_bias_ptr[i];
- calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
- STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i,
- inv_obj.compass_bias_ptr[i]);
- }
- for (i = 0; i < 3; i++) {
- uint32_t t = (uint32_t) inv_obj.compass_scale[i];
- calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(t & 0xff);
- STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]);
- }
- for (i = 0; i < 6; i++) {
- dToLL.db = inv_obj.compass_prev_xty[i];
- calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
- STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i,
- inv_obj.compass_prev_xty[i]);
- }
- for (i = 0; i < 36; i++) {
- dToLL.db = inv_obj.compass_prev_m[i];
- calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
- STORECAL_LOG("compass_prev_m[%d] = %lf\n", i,
- inv_obj.compass_prev_m[i]);
- }
-
- /* store the compass offsets and validity flag */
- calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID];
- calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0];
- calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1];
- calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2];
-
- /* store the compass accuracy */
- calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy);
-
- /* add a checksum */
- chk = inv_checksum(calData + INV_CAL_HDR_LEN,
- length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN));
- calData[ptr++] = (unsigned char)((chk >> 24) & 0xff);
- calData[ptr++] = (unsigned char)((chk >> 16) & 0xff);
- calData[ptr++] = (unsigned char)((chk >> 8) & 0xff);
- calData[ptr++] = (unsigned char)(chk & 0xff);
-
- STORECAL_LOG("Exiting inv_store_cal\n");
- return INV_SUCCESS;
-}
-
-/**
- * @brief Load a calibration file.
- *
- * @pre Must be in INV_STATE_DMP_OPENED state.
- * inv_dmp_open() or inv_dmp_stop() must have been called.
- * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- * been called.
- *
- * @return 0 or error code.
- */
-inv_error_t inv_load_calibration(void)
-{
- unsigned char *calData;
- inv_error_t result;
- unsigned int length;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- result = inv_serial_get_cal_length(&length);
- if (result == INV_ERROR_FILE_OPEN) {
- MPL_LOGI("Calibration data not loaded\n");
- return INV_SUCCESS;
- }
- if (result || length <= 0) {
- MPL_LOGE("Could not get file calibration length - "
- "error %d - aborting\n", result);
- return result;
- }
- calData = (unsigned char *)inv_malloc(length);
- if (!calData) {
- MPL_LOGE("Could not allocate buffer of %d bytes - "
- "aborting\n", length);
- return INV_ERROR_MEMORY_EXAUSTED;
- }
- result = inv_serial_read_cal(calData, length);
- if (result) {
- MPL_LOGE("Could not read the calibration data from file - "
- "error %d - aborting\n", result);
- goto free_mem_n_exit;
-
- }
- result = inv_load_cal(calData);
- if (result) {
- MPL_LOGE("Could not load the calibration data - "
- "error %d - aborting\n", result);
- goto free_mem_n_exit;
-
- }
-
-
-
-free_mem_n_exit:
- inv_free(calData);
- return INV_SUCCESS;
-}
-
-/**
- * @brief Store runtime calibration data to a file
- *
- * @pre Must be in INV_STATE_DMP_OPENED state.
- * inv_dmp_open() or inv_dmp_stop() must have been called.
- * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
- * been called.
- *
- * @return 0 or error code.
- */
-inv_error_t inv_store_calibration(void)
-{
- unsigned char *calData;
- inv_error_t result;
- unsigned int length;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- length = inv_get_cal_length();
- calData = (unsigned char *)inv_malloc(length);
- if (!calData) {
- MPL_LOGE("Could not allocate buffer of %d bytes - "
- "aborting\n", length);
- return INV_ERROR_MEMORY_EXAUSTED;
- }
- result = inv_store_cal(calData, length);
- if (result) {
- MPL_LOGE("Could not store calibrated data on file - "
- "error %d - aborting\n", result);
- goto free_mem_n_exit;
-
- }
- result = inv_serial_write_cal(calData, length);
- if (result) {
- MPL_LOGE("Could not write calibration data - " "error %d\n", result);
- goto free_mem_n_exit;
-
- }
-
-
-
-free_mem_n_exit:
- inv_free(calData);
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/ml_stored_data.h b/60xx/mlsdk/mllite/ml_stored_data.h
deleted file mode 100644
index 74c2b7c..0000000
--- a/60xx/mlsdk/mllite/ml_stored_data.h
+++ /dev/null
@@ -1,62 +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:$
- *
- ******************************************************************************/
-
-#ifndef INV_STORED_DATA_H
-#define INV_STORED_DATA_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
- Includes.
-*/
-
-#include "mltypes.h"
-
-/*
- Defines
-*/
-#define INV_CAL_ACCEL_LEN (12)
-#define INV_CAL_COMPASS_LEN (555 + 5)
-#define INV_CAL_HDR_LEN (6)
-#define INV_CAL_CHK_LEN (4)
-
-/*
- APIs
-*/
- inv_error_t inv_load_calibration(void);
- inv_error_t inv_store_calibration(void);
-
-/*
- Other prototypes
-*/
- inv_error_t inv_load_cal(unsigned char *calData);
- inv_error_t inv_store_cal(unsigned char *calData, int length);
- unsigned int inv_get_cal_length(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* INV_STORED_DATA_H */
diff --git a/60xx/mlsdk/mllite/mlarray.c b/60xx/mlsdk/mllite/mlarray.c
deleted file mode 100644
index 6ae85e0..0000000
--- a/60xx/mlsdk/mllite/mlarray.c
+++ /dev/null
@@ -1,2524 +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.c 5085 2011-04-08 22:25:14Z phickey $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML
- * @{
- * @file mlarray.c
- * @brief APIs to read different data sets from FIFO.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-#include "ml.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mlMathFunc.h"
-#include "mlmath.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "mlsupervisor.h"
-#include "mldl.h"
-#include "dmpKey.h"
-#include "compass.h"
-
-/**
- * @brief inv_get_gyro is used to get the most recent gyroscope measurement.
- * The argument array elements are ordered X,Y,Z.
- * The values are scaled at 1 dps = 2^16 LSBs.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-
- /* inv_get_gyro implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_accel is used to get the most recent
- * accelerometer measurement.
- * The argument array elements are ordered X,Y,Z.
- * The values are scaled in units of g (gravity),
- * where 1 g = 2^16 LSBs.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_accel implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_temperature is used to get the most recent
- * temperature measurement.
- * The argument array should only have one element.
- * The value is in units of deg C (degrees Celsius), where
- * 2^16 LSBs = 1 deg C.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to the data to be passed back to the user.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_temperature implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_rot_mat is used to get the rotation matrix
- * representation of the current sensor fusion solution.
- * 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>
- * Values are scaled, where 1.0 = 2^30 LSBs.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_rot_mat(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- long qdata[4];
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- inv_get_quaternion(qdata);
- inv_quaternion_to_rotation(qdata, data);
-
- return result;
-}
-
-/**
- * @brief inv_get_quaternion is used to get the quaternion representation
- * of the current sensor fusion solution.
- * The values are scaled where 1.0 = 2^30 LSBs.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 4 cells long </b>.
- *
- * @return INV_SUCCESS if the command is successful; an ML error code otherwise.
- */
- /* inv_get_quaternion implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_linear_accel is used to get an estimate of linear
- * acceleration, based on the most recent accelerometer measurement
- * and sensor fusion solution.
- * The values are scaled where 1 g (gravity) = 2^16 LSBs.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_linear_accel implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_linear_accel_in_world is used to get an estimate of
- * linear acceleration, in the world frame, based on the most
- * recent accelerometer measurement and sensor fusion solution.
- * The values are scaled where 1 g (gravity) = 2^16 LSBs.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_linear_accel_in_world implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_gravity is used to get an estimate of the body frame
- * gravity vector, based on the most recent sensor fusion solution.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_gravity implemented in mlFIFO.c */
-
-/**
- * @internal
- * @brief inv_get_angular_velocity is used to get an estimate of the body
- * frame angular velocity, which is computed from the current and
- * previous sensor fusion solutions.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_angular_velocity(long *data)
-{
-
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- /* not implemented. old (invalid) implementation:
- if ( inv_get_state() < INV_STATE_DMP_OPENED )
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- data[0] = inv_obj.ang_v_body[0];
- data[1] = inv_obj.ang_v_body[1];
- data[2] = inv_obj.ang_v_body[2];
-
- return result;
- */
-}
-
-/**
- * @brief inv_get_euler_angles is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * Euler angles may follow various conventions. This function is equivelant
- * to inv_get_euler_angles_x, refer to inv_get_euler_angles_x for more
- * information.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_euler_angles(long *data)
-{
- return inv_get_euler_angles_x(data);
-}
-
-/**
- * @brief inv_get_euler_angles_x is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * Euler angles are returned according to the X convention.
- * 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.
- * <TABLE>
- * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- * <TR><TD> 0 </TD><TD>Roll </TD><TD>X axis </TD></TR>
- * <TR><TD> 1 </TD><TD>Pitch </TD><TD>Y axis </TD></TR>
- * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
- * </TABLE>
- *
- * Values are scaled where 1.0 = 2^16.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_euler_angles_x(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_rot_mat_float(rotMatrix);
- tmp = rotMatrix[6];
- if (tmp > 1.0f) {
- tmp = 1.0f;
- }
- if (tmp < -1.0f) {
- tmp = -1.0f;
- }
- data[0] =
- (long)((float)
- (atan2f(rotMatrix[7], rotMatrix[8]) * 57.29577951308) *
- 65536L);
- data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
- data[2] =
- (long)((float)
- (atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308) *
- 65536L);
- return result;
-}
-
-/**
- * @brief inv_get_euler_angles_y is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * Euler angles are returned according to the Y convention.
- * 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.
- * <TABLE>
- * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- * <TR><TD> 0 </TD><TD>Roll </TD><TD>Y axis </TD></TR>
- * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
- * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
- * </TABLE>
- *
- * Values are scaled where 1.0 = 2^16.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_euler_angles_y(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_rot_mat_float(rotMatrix);
- tmp = rotMatrix[7];
- if (tmp > 1.0f) {
- tmp = 1.0f;
- }
- if (tmp < -1.0f) {
- tmp = -1.0f;
- }
- data[0] =
- (long)((float)
- (atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308f) *
- 65536L);
- data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
- data[2] =
- (long)((float)
- (atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308f) *
- 65536L);
- return result;
-}
-
-/** @brief inv_get_euler_angles_z is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * 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.
- * Euler angles are returned according to the Y convention.
- * <TABLE>
- * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- * <TR><TD> 0 </TD><TD>Roll </TD><TD>Z axis </TD></TR>
- * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
- * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Y axis </TD></TR>
- * </TABLE>
- *
- * Values are scaled where 1.0 = 2^16.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-
-inv_error_t inv_get_euler_angles_z(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_rot_mat_float(rotMatrix);
- tmp = rotMatrix[8];
- if (tmp > 1.0f) {
- tmp = 1.0f;
- }
- if (tmp < -1.0f) {
- tmp = -1.0f;
- }
- data[0] =
- (long)((float)
- (atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308) *
- 65536L);
- data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L);
- data[2] =
- (long)((float)
- (atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308) *
- 65536L);
- return result;
-}
-
-/**
- * @brief inv_get_gyro_temp_slope is used to get is used to get the temperature
- * compensation algorithm's estimate of the gyroscope bias temperature
- * coefficient.
- * The argument array elements are ordered X,Y,Z.
- * Values are in units of dps per deg C (degrees per second per degree
- * Celcius). Values are scaled so that 1 dps per deg C = 2^16 LSBs.
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_gyro_temp_slope(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
- data[0] = (long)(inv_obj.x_gyro_coef[1] * 65536.0f);
- data[1] = (long)(inv_obj.y_gyro_coef[1] * 65536.0f);
- data[2] = (long)(inv_obj.z_gyro_coef[1] * 65536.0f);
- } else {
- data[0] = inv_obj.temp_slope[0];
- data[1] = inv_obj.temp_slope[1];
- data[2] = inv_obj.temp_slope[2];
- }
- return result;
-}
-
-/**
- * @brief inv_get_gyro_bias is used to get the gyroscope biases.
- * The argument array elements are ordered X,Y,Z.
- * The values are scaled such that 1 dps = 2^16 LSBs.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_gyro_bias(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- data[0] = inv_obj.gyro_bias[0];
- data[1] = inv_obj.gyro_bias[1];
- data[2] = inv_obj.gyro_bias[2];
-
- return result;
-}
-
-/**
- * @brief inv_get_accel_bias is used to get the accelerometer baises.
- * The argument array elements are ordered X,Y,Z.
- * The values are scaled such that 1 g (gravity) = 2^16 LSBs.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_accel_bias(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- data[0] = inv_obj.accel_bias[0];
- data[1] = inv_obj.accel_bias[1];
- data[2] = inv_obj.accel_bias[2];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_bias is used to get Magnetometer Bias
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_bias(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- data[0] =
- inv_obj.compass_bias[0] +
- (long)((long long)inv_obj.init_compass_bias[0] * inv_obj.compass_sens /
- 16384);
- data[1] =
- inv_obj.compass_bias[1] +
- (long)((long long)inv_obj.init_compass_bias[1] * inv_obj.compass_sens /
- 16384);
- data[2] =
- inv_obj.compass_bias[2] +
- (long)((long long)inv_obj.init_compass_bias[2] * inv_obj.compass_sens /
- 16384);
-
- return result;
-}
-
-/**
- * @brief inv_get_gyro_and_accel_sensor is used to get the most recent set of all sensor data.
- * The argument array elements are ordered gyroscope X,Y, and Z,
- * accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
- * \if UMPL The magnetometer elements are not populated in UMPL. \endif
- * The gyroscope and accelerometer data is not scaled or offset, it is
- * copied directly from the sensor registers.
- * In the case of accelerometers with 8-bit output resolution, the data
- * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
- * full scale range
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
- /* inv_get_gyro_and_accel_sensor implemented in mlFIFO.c */
-
-/**
- * @cond MPL
- * @brief inv_get_mag_raw_data is used to get Raw magnetometer data.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_raw_data(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.compass_sensor_data[0];
- data[1] = inv_obj.compass_sensor_data[1];
- data[2] = inv_obj.compass_sensor_data[2];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_magnetometer is used to get magnetometer data.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_magnetometer(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0];
- data[1] = inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1];
- data[2] = inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_pressure is used to get Pressure data.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to data to be passed back to the user.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_pressure(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.pressure;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_heading is used to get heading from Rotation Matrix.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to data to be passed back to the user.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-
-inv_error_t inv_get_heading(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- result = inv_get_rot_mat_float(rotMatrix);
- if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
- tmp =
- (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
- 90.0f);
- } else {
- tmp =
- (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
- 90.0f);
- }
- if (tmp < 0) {
- tmp += 360.0f;
- }
- data[0] = (long)((360 - tmp) * 65536.0f);
-
- return result;
-}
-
-/**
- * @brief inv_get_gyro_cal_matrix is used to get the gyroscope
- * calibration matrix. The gyroscope calibration matrix defines the relationship
- * between the gyroscope sensor axes and the sensor fusion solution axes.
- * Calibration matrix data members will have a value of 1, 0, or -1.
- * The matrix has members
- * <center>C11 C12 C13</center>
- * <center>C21 C22 C23</center>
- * <center>C31 C32 C33</center>
- * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_gyro_cal_matrix(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.gyro_cal[0];
- data[1] = inv_obj.gyro_cal[1];
- data[2] = inv_obj.gyro_cal[2];
- data[3] = inv_obj.gyro_cal[3];
- data[4] = inv_obj.gyro_cal[4];
- data[5] = inv_obj.gyro_cal[5];
- data[6] = inv_obj.gyro_cal[6];
- data[7] = inv_obj.gyro_cal[7];
- data[8] = inv_obj.gyro_cal[8];
-
- return result;
-}
-
-/**
- * @brief inv_get_accel_cal_matrix is used to get the accelerometer
- * calibration matrix.
- * Calibration matrix data members will have a value of 1, 0, or -1.
- * The matrix has members
- * <center>C11 C12 C13</center>
- * <center>C21 C22 C23</center>
- * <center>C31 C32 C33</center>
- * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_accel_cal_matrix(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.accel_cal[0];
- data[1] = inv_obj.accel_cal[1];
- data[2] = inv_obj.accel_cal[2];
- data[3] = inv_obj.accel_cal[3];
- data[4] = inv_obj.accel_cal[4];
- data[5] = inv_obj.accel_cal[5];
- data[6] = inv_obj.accel_cal[6];
- data[7] = inv_obj.accel_cal[7];
- data[8] = inv_obj.accel_cal[8];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_cal_matrix is used to get magnetometer calibration matrix.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @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.
- * @endcond
- */
-inv_error_t inv_get_mag_cal_matrix(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.compass_cal[0];
- data[1] = inv_obj.compass_cal[1];
- data[2] = inv_obj.compass_cal[2];
- data[3] = inv_obj.compass_cal[3];
- data[4] = inv_obj.compass_cal[4];
- data[5] = inv_obj.compass_cal[5];
- data[6] = inv_obj.compass_cal[6];
- data[7] = inv_obj.compass_cal[7];
- data[8] = inv_obj.compass_cal[8];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_bias_error is used to get magnetometer Bias error.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long at least</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_bias_error(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- if (inv_obj.large_field == 0) {
- data[0] = inv_obj.compass_bias_error[0];
- data[1] = inv_obj.compass_bias_error[1];
- data[2] = inv_obj.compass_bias_error[2];
- } else {
- data[0] = P_INIT;
- data[1] = P_INIT;
- data[2] = P_INIT;
- }
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_scale is used to get magnetometer scale.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long at least</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_scale(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.compass_scale[0];
- data[1] = inv_obj.compass_scale[1];
- data[2] = inv_obj.compass_scale[2];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_local_field is used to get local magnetic field data.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long at least</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_local_field(long *data)
-{
- inv_error_t result = INV_SUCCESS;
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = inv_obj.local_field[0];
- data[1] = inv_obj.local_field[1];
- data[2] = inv_obj.local_field[2];
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_relative_quaternion is used to get relative quaternion.
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 4 cells long at least</b>.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- * @endcond
- */
-/* inv_get_relative_quaternion implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_gyro_float is used to get the most recent gyroscope measurement.
- * The argument array elements are ordered X,Y,Z.
- * The values are in units of dps (degrees per second).
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- long ldata[3];
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- result = inv_get_gyro(ldata);
- data[0] = (float)ldata[0] / 65536.0f;
- data[1] = (float)ldata[1] / 65536.0f;
- data[2] = (float)ldata[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @internal
- * @brief inv_get_angular_velocity_float is used to get 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.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_angular_velocity_float(float *data)
-{
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- /* not implemented. old (invalid) implementation:
- return inv_get_gyro_float(data);
- */
-}
-
-/**
- * @brief inv_get_accel_float is used to get the most recent accelerometer measurement.
- * The argument array elements are ordered X,Y,Z.
- * The values are in units of g (gravity).
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
- /* inv_get_accel_float implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_temperature_float is used to get the most recent
- * temperature measurement.
- * The argument array should only have one element.
- * The value is in units of deg C (degrees Celsius).
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to data to be passed back to the user.
- *
- * @return Zero if the command is successful; an ML error code otherwise.
- */
-inv_error_t inv_get_temperature_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- long ldata[1];
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_temperature(ldata);
- data[0] = (float)ldata[0] / 65536.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_rot_mat_float is used to get an array of nine data points representing the rotation
- * matrix generated from all available sensors.
- * 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>.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @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_rot_mat_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- {
- long qdata[4], rdata[9];
- inv_get_quaternion(qdata);
- inv_quaternion_to_rotation(qdata, rdata);
- data[0] = (float)rdata[0] / 1073741824.0f;
- data[1] = (float)rdata[1] / 1073741824.0f;
- data[2] = (float)rdata[2] / 1073741824.0f;
- data[3] = (float)rdata[3] / 1073741824.0f;
- data[4] = (float)rdata[4] / 1073741824.0f;
- data[5] = (float)rdata[5] / 1073741824.0f;
- data[6] = (float)rdata[6] / 1073741824.0f;
- data[7] = (float)rdata[7] / 1073741824.0f;
- data[8] = (float)rdata[8] / 1073741824.0f;
- }
-
- return result;
-}
-
-/**
- * @brief inv_get_quaternion_float is used to get the quaternion representation
- * of the current sensor fusion solution.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 4 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an ML error code otherwise.
- */
- /* inv_get_quaternion_float implemented in mlFIFO.c */
-
-/**
- * @brief inv_get_linear_accel_float is used to get an estimate of linear
- * acceleration, based on the most recent accelerometer measurement
- * and sensor fusion solution.
- * The values are in units of g (gravity).
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_linear_accel_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- long ldata[3];
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_linear_accel(ldata);
- data[0] = (float)ldata[0] / 65536.0f;
- data[1] = (float)ldata[1] / 65536.0f;
- data[2] = (float)ldata[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_linear_accel_in_world_float is used to get an estimate of
- * linear acceleration, in the world frame, based on the most
- * recent accelerometer measurement and sensor fusion solution.
- * The values are in units of g (gravity).
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_linear_accel_in_world_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- long ldata[3];
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_linear_accel_in_world(ldata);
- data[0] = (float)ldata[0] / 65536.0f;
- data[1] = (float)ldata[1] / 65536.0f;
- data[2] = (float)ldata[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_gravity_float is used to get an estimate of the body frame
- * gravity vector, based on the most recent sensor fusion solution.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long at least</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gravity_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- long ldata[3];
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- result = inv_get_gravity(ldata);
- data[0] = (float)ldata[0] / 65536.0f;
- data[1] = (float)ldata[1] / 65536.0f;
- data[2] = (float)ldata[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_gyro_cal_matrix_float is used to get the gyroscope
- * calibration matrix. The gyroscope calibration matrix defines the relationship
- * between the gyroscope sensor axes and the sensor fusion solution axes.
- * Calibration matrix data members will have a value of 1.0, 0, or -1.0.
- * The matrix has members
- * <center>C11 C12 C13</center>
- * <center>C21 C22 C23</center>
- * <center>C31 C32 C33</center>
- * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_cal_matrix_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.gyro_cal[0] / 1073741824.0f;
- data[1] = (float)inv_obj.gyro_cal[1] / 1073741824.0f;
- data[2] = (float)inv_obj.gyro_cal[2] / 1073741824.0f;
- data[3] = (float)inv_obj.gyro_cal[3] / 1073741824.0f;
- data[4] = (float)inv_obj.gyro_cal[4] / 1073741824.0f;
- data[5] = (float)inv_obj.gyro_cal[5] / 1073741824.0f;
- data[6] = (float)inv_obj.gyro_cal[6] / 1073741824.0f;
- data[7] = (float)inv_obj.gyro_cal[7] / 1073741824.0f;
- data[8] = (float)inv_obj.gyro_cal[8] / 1073741824.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_accel_cal_matrix_float is used to get the accelerometer
- * calibration matrix.
- * Calibration matrix data members will have a value of 1.0, 0, or -1.0.
- * The matrix has members
- * <center>C11 C12 C13</center>
- * <center>C21 C22 C23</center>
- * <center>C31 C32 C33</center>
- * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-
-inv_error_t inv_get_accel_cal_matrix_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.accel_cal[0] / 1073741824.0f;
- data[1] = (float)inv_obj.accel_cal[1] / 1073741824.0f;
- data[2] = (float)inv_obj.accel_cal[2] / 1073741824.0f;
- data[3] = (float)inv_obj.accel_cal[3] / 1073741824.0f;
- data[4] = (float)inv_obj.accel_cal[4] / 1073741824.0f;
- data[5] = (float)inv_obj.accel_cal[5] / 1073741824.0f;
- data[6] = (float)inv_obj.accel_cal[6] / 1073741824.0f;
- data[7] = (float)inv_obj.accel_cal[7] / 1073741824.0f;
- data[8] = (float)inv_obj.accel_cal[8] / 1073741824.0f;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_cal_matrix_float is used to get 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>
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @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.
- * @endcond
- */
-inv_error_t inv_get_mag_cal_matrix_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.compass_cal[0] / 1073741824.0f;
- data[1] = (float)inv_obj.compass_cal[1] / 1073741824.0f;
- data[2] = (float)inv_obj.compass_cal[2] / 1073741824.0f;
- data[3] = (float)inv_obj.compass_cal[3] / 1073741824.0f;
- data[4] = (float)inv_obj.compass_cal[4] / 1073741824.0f;
- data[5] = (float)inv_obj.compass_cal[5] / 1073741824.0f;
- data[6] = (float)inv_obj.compass_cal[6] / 1073741824.0f;
- data[7] = (float)inv_obj.compass_cal[7] / 1073741824.0f;
- data[8] = (float)inv_obj.compass_cal[8] / 1073741824.0f;
- return result;
-}
-
-/**
- * @brief inv_get_gyro_temp_slope_float is used to get the temperature
- * compensation algorithm's estimate of the gyroscope bias temperature
- * coefficient.
- * The argument array elements are ordered X,Y,Z.
- * Values are in units of dps per deg C (degrees per second per degree
- * Celcius)
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long </b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_temp_slope_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) {
- data[0] = inv_obj.x_gyro_coef[1];
- data[1] = inv_obj.y_gyro_coef[1];
- data[2] = inv_obj.z_gyro_coef[1];
- } else {
- data[0] = (float)inv_obj.temp_slope[0] / 65536.0f;
- data[1] = (float)inv_obj.temp_slope[1] / 65536.0f;
- data[2] = (float)inv_obj.temp_slope[2] / 65536.0f;
- }
-
- return result;
-}
-
-/**
- * @brief inv_get_gyro_bias_float is used to get the gyroscope biases.
- * The argument array elements are ordered X,Y,Z.
- * The values are in units of dps (degrees per second).
-
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_bias_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.gyro_bias[0] / 65536.0f;
- data[1] = (float)inv_obj.gyro_bias[1] / 65536.0f;
- data[2] = (float)inv_obj.gyro_bias[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_accel_bias_float is used to get the accelerometer baises.
- * The argument array elements are ordered X,Y,Z.
- * The values are in units of g (gravity).
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_accel_bias_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.accel_bias[0] / 65536.0f;
- data[1] = (float)inv_obj.accel_bias[1] / 65536.0f;
- data[2] = (float)inv_obj.accel_bias[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_bias_float is used to get an array of three data points representing
- * the compass biases.
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_bias_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] =
- ((float)
- (inv_obj.compass_bias[0] +
- (long)((long long)inv_obj.init_compass_bias[0] *
- inv_obj.compass_sens / 16384))) / 65536.0f;
- data[1] =
- ((float)
- (inv_obj.compass_bias[1] +
- (long)((long long)inv_obj.init_compass_bias[1] *
- inv_obj.compass_sens / 16384))) / 65536.0f;
- data[2] =
- ((float)
- (inv_obj.compass_bias[2] +
- (long)((long long)inv_obj.init_compass_bias[2] *
- inv_obj.compass_sens / 16384))) / 65536.0f;
-
- return result;
-}
-
-/**
- * @brief inv_get_gyro_and_accel_sensor_float is used to get the most recent set of all sensor data.
- * The argument array elements are ordered gyroscope X,Y, and Z,
- * accelerometer X, Y, and Z, and magnetometer X,Y, and Z.
- * \if UMPL The magnetometer elements are not populated in UMPL. \endif
- * The gyroscope and accelerometer data is not scaled or offset, it is
- * copied directly from the sensor registers, and cast as a float.
- * In the case of accelerometers with 8-bit output resolution, the data
- * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g
- * full scale range
-
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 9 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_gyro_and_accel_sensor_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- long ldata[6];
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_gyro_and_accel_sensor(ldata);
- data[0] = (float)ldata[0];
- data[1] = (float)ldata[1];
- data[2] = (float)ldata[2];
- data[3] = (float)ldata[3];
- data[4] = (float)ldata[4];
- data[5] = (float)ldata[5];
- data[6] = (float)inv_obj.compass_sensor_data[0];
- data[7] = (float)inv_obj.compass_sensor_data[1];
- data[8] = (float)inv_obj.compass_sensor_data[2];
-
- return result;
-}
-
-/**
- * @brief inv_get_euler_angles_x is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * Euler angles are returned according to the X convention.
- * 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.
- * <TABLE>
- * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- * <TR><TD> 0 </TD><TD>Roll </TD><TD>X axis </TD></TR>
- * <TR><TD> 1 </TD><TD>Pitch </TD><TD>Y axis </TD></TR>
- * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
- *
- </TABLE>
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_x_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_rot_mat_float(rotMatrix);
- tmp = rotMatrix[6];
- if (tmp > 1.0f) {
- tmp = 1.0f;
- }
- if (tmp < -1.0f) {
- tmp = -1.0f;
- }
- data[0] =
- (float)(atan2f(rotMatrix[7],
- rotMatrix[8]) * 57.29577951308);
- data[1] = (float)((double)asin(tmp) * 57.29577951308);
- data[2] =
- (float)(atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308);
-
- return result;
-}
-
-/**
- * @brief inv_get_euler_angles_float is used to get an array of three data points 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.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_float(float *data)
-{
- return inv_get_euler_angles_x_float(data);
-}
-
-/** @brief inv_get_euler_angles_y_float is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * Euler angles are returned according to the Y convention.
- * 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.
- * <TABLE>
- * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- * <TR><TD> 0 </TD><TD>Roll </TD><TD>Y axis </TD></TR>
- * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
- * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR>
- * </TABLE>
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_y_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_rot_mat_float(rotMatrix);
- tmp = rotMatrix[7];
- if (tmp > 1.0f) {
- tmp = 1.0f;
- }
- if (tmp < -1.0f) {
- tmp = -1.0f;
- }
- data[0] =
- (float)(atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308);
- data[1] = (float)((double)asin(tmp) * 57.29577951308);
- data[2] =
- (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308);
-
- return result;
-}
-
-/** @brief inv_get_euler_angles_z_float is used to get the Euler angle representation
- * of the current sensor fusion solution.
- * 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.
- * Euler angles are returned according to the Y convention.
- * <TABLE>
- * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR>
- * <TR><TD> 0 </TD><TD>Roll </TD><TD>Z axis </TD></TR>
- * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR>
- * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Y axis </TD></TR>
- * </TABLE>
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- */
-inv_error_t inv_get_euler_angles_z_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- result = inv_get_rot_mat_float(rotMatrix);
- tmp = rotMatrix[8];
- if (tmp > 1.0f) {
- tmp = 1.0f;
- }
- if (tmp < -1.0f) {
- tmp = -1.0f;
- }
- data[0] =
- (float)(atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308);
- data[1] = (float)((double)asin(tmp) * 57.29577951308);
- data[2] =
- (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308);
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_raw_data_float is used to get Raw magnetometer data
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_raw_data_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] =
- (float)(inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]);
- data[1] =
- (float)(inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]);
- data[2] =
- (float)(inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]);
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_magnetometer_float is used to get magnetometer data
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_magnetometer_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.compass_calibrated_data[0] / 65536.0f;
- data[1] = (float)inv_obj.compass_calibrated_data[1] / 65536.0f;
- data[2] = (float)inv_obj.compass_calibrated_data[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_pressure_float is used to get a single value representing the pressure in Pascal
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to the data to be passed back to the user.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_pressure_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.pressure;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_heading_float is used to get 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.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to the data to be passed back to the user.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_heading_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- float rotMatrix[9];
- float tmp;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- inv_get_rot_mat_float(rotMatrix);
- if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) {
- tmp =
- (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 -
- 90.0f);
- } else {
- tmp =
- (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 +
- 90.0f);
- }
- if (tmp < 0) {
- tmp += 360.0f;
- }
- data[0] = 360 - tmp;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_bias_error_float is used to get 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() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_bias_error_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- if (inv_obj.large_field == 0) {
- data[0] = (float)inv_obj.compass_bias_error[0];
- data[1] = (float)inv_obj.compass_bias_error[1];
- data[2] = (float)inv_obj.compass_bias_error[2];
- } else {
- data[0] = (float)P_INIT;
- data[1] = (float)P_INIT;
- data[2] = (float)P_INIT;
- }
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_mag_scale_float is used to get magnetometer scale.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_mag_scale_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.compass_scale[0] / 65536.0f;
- data[1] = (float)inv_obj.compass_scale[1] / 65536.0f;
- data[2] = (float)inv_obj.compass_scale[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_local_field_float is used to get local magnetic field data.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 3 cells long</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_local_field_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.local_field[0] / 65536.0f;
- data[1] = (float)inv_obj.local_field[1] / 65536.0f;
- data[2] = (float)inv_obj.local_field[2] / 65536.0f;
-
- return result;
-}
-
-/**
- * @cond MPL
- * @brief inv_get_relative_quaternion_float is used to get relative quaternion data.
- *
- * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif
- * must have been called.
- *
- * @param data
- * A pointer to an array to be passed back to the user.
- * <b>Must be 4 cells long at least</b>.
- *
- * @return INV_SUCCESS if the command is successful; an error code otherwise.
- * @endcond
- */
-inv_error_t inv_get_relative_quaternion_float(float *data)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
-
- if (inv_get_state() < INV_STATE_DMP_OPENED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- data[0] = (float)inv_obj.relative_quat[0] / 1073741824.0f;
- data[1] = (float)inv_obj.relative_quat[1] / 1073741824.0f;
- data[2] = (float)inv_obj.relative_quat[2] / 1073741824.0f;
- data[3] = (float)inv_obj.relative_quat[3] / 1073741824.0f;
-
- return result;
-}
-
-/**
- * Returns the curren compass accuracy.
- *
- * - 0: Unknown: The accuracy is unreliable and compass data should not be used
- * - 1: Low: The compass accuracy is low.
- * - 2: Medium: The compass accuracy is medium.
- * - 3: High: The compas acurracy is high and can be trusted
- *
- * @param accuracy The accuracy level in the range 0-3
- *
- * @return ML_SUCCESS or non-zero error code
- */
-inv_error_t inv_get_compass_accuracy(int *accuracy)
-{
- if (inv_get_state() != INV_STATE_DMP_STARTED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- *accuracy = inv_obj.compass_accuracy;
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_set_gyro_bias is used to set the gyroscope bias.
- * The argument array elements are ordered X,Y,Z.
- * The values are scaled at 1 dps = 2^16 LSBs.
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- *
- * @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_gyro_bias(long *data)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
- long biasTmp;
- long sf = 0;
- short offset[GYRO_NUM_AXES];
- int i;
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- if (mldl_cfg->gyro_sens_trim != 0) {
- sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
- } else {
- sf = 2000;
- }
- for (i = 0; i < GYRO_NUM_AXES; i++) {
- inv_obj.gyro_bias[i] = data[i];
- biasTmp = -inv_obj.gyro_bias[i] / sf;
- if (biasTmp < 0)
- biasTmp += 65536L;
- offset[i] = (short)biasTmp;
- }
- result = inv_set_offset(offset);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_set_accel_bias is used to set the accelerometer bias.
- * The argument array elements are ordered X,Y,Z.
- * The values are scaled in units of g (gravity),
- * where 1 g = 2^16 LSBs.
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- *
- * @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_accel_bias(long *data)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
- long biasTmp;
- int i, j;
- unsigned char regs[6];
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
-
- for (i = 0; i < ACCEL_NUM_AXES; i++) {
- inv_obj.accel_bias[i] = data[i];
- if (inv_obj.accel_sens != 0 && mldl_cfg && mldl_cfg->pdata) {
- long long tmp64;
- inv_obj.scaled_accel_bias[i] = 0;
- for (j = 0; j < ACCEL_NUM_AXES; j++) {
- inv_obj.scaled_accel_bias[i] +=
- data[j] *
- (long)mldl_cfg->pdata->accel.orientation[i * 3 + j];
- }
- tmp64 = (long long)inv_obj.scaled_accel_bias[i] << 13;
- biasTmp = (long)(tmp64 / inv_obj.accel_sens);
- } else {
- biasTmp = 0;
- }
- if (biasTmp < 0)
- biasTmp += 65536L;
- regs[2 * i + 0] = (unsigned char)(biasTmp / 256);
- regs[2 * i + 1] = (unsigned char)(biasTmp % 256);
- }
- result = inv_set_mpu_memory(KEY_D_1_8, 2, ®s[0]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_mpu_memory(KEY_D_1_10, 2, ®s[2]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_set_mpu_memory(KEY_D_1_2, 2, ®s[4]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return INV_SUCCESS;
-}
-
-/**
- * @cond MPL
- * @brief inv_set_mag_bias is used to set Compass Bias
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @param data A pointer to an array to be copied from the user.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- * @endcond
- */
-inv_error_t inv_set_mag_bias(long *data)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- inv_set_compass_bias(data);
- inv_obj.init_compass_bias[0] = 0;
- inv_obj.init_compass_bias[1] = 0;
- inv_obj.init_compass_bias[2] = 0;
- inv_obj.got_compass_bias = 1;
- inv_obj.got_init_compass_bias = 1;
- inv_obj.compass_state = SF_STARTUP_SETTLE;
-
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_set_gyro_temp_slope is used to set the temperature
- * compensation algorithm's estimate of the gyroscope bias temperature
- * coefficient.
- * The argument array elements are ordered X,Y,Z.
- * Values are in units of dps per deg C (degrees per second per degree
- * Celcius), and scaled such that 1 dps per deg C = 2^16 LSBs.
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document.
- *
- * @brief inv_set_gyro_temp_slope is used to set Gyro temperature slope
- *
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- *
- * @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_gyro_temp_slope(long *data)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
- int i;
- long sf;
- unsigned char regs[3];
-
- inv_obj.factory_temp_comp = 1;
- inv_obj.temp_slope[0] = data[0];
- inv_obj.temp_slope[1] = data[1];
- inv_obj.temp_slope[2] = data[2];
- for (i = 0; i < GYRO_NUM_AXES; i++) {
- sf = -inv_obj.temp_slope[i] / 1118;
- if (sf > 127) {
- sf -= 256;
- }
- regs[i] = (unsigned char)sf;
- }
- result = inv_set_offsetTC(regs);
-
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @cond MPL
- * @brief inv_set_local_field is used to set local magnetic field
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @param data A pointer to an array to be copied from the user.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- * @endcond
- */
-inv_error_t inv_set_local_field(long *data)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- inv_obj.local_field[0] = data[0];
- inv_obj.local_field[1] = data[1];
- inv_obj.local_field[2] = data[2];
- inv_obj.new_local_field = 1;
-
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @cond MPL
- * @brief inv_set_mag_scale is used to set magnetometer scale
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @param data A pointer to an array to be copied from the user.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- * @endcond
- */
-inv_error_t inv_set_mag_scale(long *data)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- inv_obj.compass_scale[0] = data[0];
- inv_obj.compass_scale[1] = data[1];
- inv_obj.compass_scale[2] = data[2];
-
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_set_gyro_temp_slope_float is used to get the temperature
- * compensation algorithm's estimate of the gyroscope bias temperature
- * coefficient.
- * The argument array elements are ordered X,Y,Z.
- * Values are in units of dps per deg C (degrees per second per degree
- * Celcius)
-
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- *
- * @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_gyro_temp_slope_float(float *data)
-{
- long arrayTmp[3];
- arrayTmp[0] = (long)(data[0] * 65536.f);
- arrayTmp[1] = (long)(data[1] * 65536.f);
- arrayTmp[2] = (long)(data[2] * 65536.f);
- return inv_set_gyro_temp_slope(arrayTmp);
-}
-
-/**
- * @brief inv_set_gyro_bias_float is used to set the gyroscope bias.
- * The argument array elements are ordered X,Y,Z.
- * The values are in units of dps (degrees per second).
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @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_gyro_bias_float(float *data)
-{
- long arrayTmp[3];
- arrayTmp[0] = (long)(data[0] * 65536.f);
- arrayTmp[1] = (long)(data[1] * 65536.f);
- arrayTmp[2] = (long)(data[2] * 65536.f);
- return inv_set_gyro_bias(arrayTmp);
-
-}
-
-/**
- * @brief inv_set_accel_bias_float is used to set the accelerometer bias.
- * The argument array elements are ordered X,Y,Z.
- * The values are in units of g (gravity).
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @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_accel_bias_float(float *data)
-{
- long arrayTmp[3];
- arrayTmp[0] = (long)(data[0] * 65536.f);
- arrayTmp[1] = (long)(data[1] * 65536.f);
- arrayTmp[2] = (long)(data[2] * 65536.f);
- return inv_set_accel_bias(arrayTmp);
-
-}
-
-/**
- * @cond MPL
- * @brief inv_set_mag_bias_float is used to set compass bias
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen()\ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @param data A pointer to an array to be copied from the user.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- * @endcond
- */
-inv_error_t inv_set_mag_bias_float(float *data)
-{
- long arrayTmp[3];
- arrayTmp[0] = (long)(data[0] * 65536.f);
- arrayTmp[1] = (long)(data[1] * 65536.f);
- arrayTmp[2] = (long)(data[2] * 65536.f);
- return inv_set_mag_bias(arrayTmp);
-}
-
-/**
- * @cond MPL
- * @brief inv_set_local_field_float is used to set local magnetic field
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @param data A pointer to an array to be copied from the user.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- * @endcond
- */
-inv_error_t inv_set_local_field_float(float *data)
-{
- long arrayTmp[3];
- arrayTmp[0] = (long)(data[0] * 65536.f);
- arrayTmp[1] = (long)(data[1] * 65536.f);
- arrayTmp[2] = (long)(data[2] * 65536.f);
- return inv_set_local_field(arrayTmp);
-}
-
-/**
- * @cond MPL
- * @brief inv_set_mag_scale_float is used to set magnetometer scale
- *
- * Please refer to the provided "9-Axis Sensor Fusion
- * Application Note" document provided.
- *
- * @pre MLDmpOpen() \ifnot UMPL or
- * MLDmpPedometerStandAloneOpen() \endif
- * @pre MLDmpStart() must <b>NOT</b> have been called.
- *
- * @param data A pointer to an array to be copied from the user.
- *
- * @return INV_SUCCESS if successful; a non-zero error code otherwise.
- * @endcond
- */
-inv_error_t inv_set_mag_scale_float(float *data)
-{
- long arrayTmp[3];
- arrayTmp[0] = (long)(data[0] * 65536.f);
- arrayTmp[1] = (long)(data[1] * 65536.f);
- arrayTmp[2] = (long)(data[2] * 65536.f);
- return inv_set_mag_scale(arrayTmp);
-}
diff --git a/60xx/mlsdk/mllite/mlarray_legacy.c b/60xx/mlsdk/mllite/mlarray_legacy.c
deleted file mode 100644
index 9dce8f3..0000000
--- a/60xx/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/60xx/mlsdk/mllite/mlcontrol.c b/60xx/mlsdk/mllite/mlcontrol.c
deleted file mode 100644
index bd186fa..0000000
--- a/60xx/mlsdk/mllite/mlcontrol.c
+++ /dev/null
@@ -1,797 +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: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- * @defgroup CONTROL
- * @brief Motion Library - Control Engine.
- * The Control Library processes gyroscopes, accelerometers, and
- * compasses to provide control signals that can be used in user
- * interfaces.
- * These signals can be used to manipulate objects such as documents,
- * images, cursors, menus, etc.
- *
- * @{
- * @file mlcontrol.c
- * @brief The Control Library.
- *
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mltypes.h"
-#include "ml.h"
-#include "mlos.h"
-#include "mlsl.h"
-#include "mldl.h"
-#include "mlcontrol.h"
-#include "dmpKey.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "string.h"
-
-/* - Global Vars. - */
-struct control_params cntrl_params = {
- {
- MLCTRL_SENSITIVITY_0_DEFAULT,
- MLCTRL_SENSITIVITY_1_DEFAULT,
- MLCTRL_SENSITIVITY_2_DEFAULT,
- MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity
- MLCTRL_FUNCTIONS_DEFAULT, // functions
- {
- MLCTRL_PARAMETER_ARRAY_0_DEFAULT,
- MLCTRL_PARAMETER_ARRAY_1_DEFAULT,
- MLCTRL_PARAMETER_ARRAY_2_DEFAULT,
- MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray
- {
- MLCTRL_PARAMETER_AXIS_0_DEFAULT,
- MLCTRL_PARAMETER_AXIS_1_DEFAULT,
- MLCTRL_PARAMETER_AXIS_2_DEFAULT,
- MLCTRL_PARAMETER_AXIS_3_DEFAULT}, // parameterAxis
- {
- MLCTRL_GRID_THRESHOLD_0_DEFAULT,
- MLCTRL_GRID_THRESHOLD_1_DEFAULT,
- MLCTRL_GRID_THRESHOLD_2_DEFAULT,
- MLCTRL_GRID_THRESHOLD_3_DEFAULT}, // gridThreshold
- {
- MLCTRL_GRID_MAXIMUM_0_DEFAULT,
- MLCTRL_GRID_MAXIMUM_1_DEFAULT,
- MLCTRL_GRID_MAXIMUM_2_DEFAULT,
- MLCTRL_GRID_MAXIMUM_3_DEFAULT}, // gridMaximum
- MLCTRL_GRID_CALLBACK_DEFAULT // gridCallback
-};
-
-/* - Extern Vars. - */
-struct control_obj cntrl_obj;
-extern const unsigned char *dmpConfig1;
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/**
- * @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
- * 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 gridNum A pointer to pass gridNum info back to the user.
- * @param gridChange A pointer to pass gridChange info back to the user.
- *
- * @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)
-{
- INVENSENSE_FUNC_START;
- int_fast8_t i = 0;
-
- if (inv_get_state() != INV_STATE_DMP_STARTED)
- return INV_ERROR_SM_IMPROPER_STATE;
-
- for (i = 0; i < 4; i++) {
- controlSignal[i] = cntrl_obj.controlInt[i];
- gridNum[i] = cntrl_obj.gridNum[i];
- gridChange[i] = cntrl_obj.gridChange[i];
- }
- return INV_SUCCESS;
-}
-
-/**
- * @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)
-{
- 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/60xx/mlsdk/mllite/mlcontrol.h b/60xx/mlsdk/mllite/mlcontrol.h
deleted file mode 100644
index a834fc6..0000000
--- a/60xx/mlsdk/mllite/mlcontrol.h
+++ /dev/null
@@ -1,217 +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.
- $
- */
-/*******************************************************************************
- *
- * $RCSfile: mlcontrol.h,v $
- *
- * $Date: 2011-06-10 20:13:08 -0700 (Fri, 10 Jun 2011) $
- *
- * $Revision: 5629 $
- *
- *******************************************************************************/
-
-/*******************************************************************************/
-/** @defgroup INV_CONTROL
-
- The Control processes gyroscopes and accelerometers to provide control
- signals that can be used in user interfaces to manipulate objects such as
- documents, images, cursors, menus, etc.
-
- @{
- @file mlcontrol.h
- @brief Header file for the Control Library.
-*/
-/******************************************************************************/
-#ifndef MLCONTROL_H
-#define MLCONTROL_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "mltypes.h"
-#include "ml.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlcontrol_legacy.h"
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- /*******************************************************************************/
- /* Control Signals. */
- /*******************************************************************************/
-
-#define INV_CONTROL_1 0x0001
-#define INV_CONTROL_2 0x0002
-#define INV_CONTROL_3 0x0004
-#define INV_CONTROL_4 0x0008
-
- /*******************************************************************************/
- /* Control Functions. */
- /*******************************************************************************/
-
-#define INV_GRID 0x0001 // Indicates that the user will be controlling a system that
- // has discrete steps, such as icons, menu entries, pixels, etc.
-#define INV_SMOOTH 0x0002 // Indicates that noise from unintentional motion should be filtered out.
-#define INV_DEAD_ZONE 0x0004 // Indicates that a dead zone should be used, below which sensor data is set to zero.
-#define INV_HYSTERESIS 0x0008 // Indicates that, when INV_GRID is selected, hysteresis should be used to prevent
- // the control signal from switching rapidly across elements of the grid.</dd>
-
- /*******************************************************************************/
- /* Integral reset options. */
- /*******************************************************************************/
-
-#define INV_NO_RESET 0x0000
-#define INV_RESET 0x0001
-
- /*******************************************************************************/
- /* Data select options. */
- /*******************************************************************************/
-
-#define INV_CTRL_SIGNAL 0x0000
-#define INV_CTRL_GRID_NUM 0x0001
-
- /*******************************************************************************/
- /* Control Axis. */
- /*******************************************************************************/
-#define INV_CTRL_PITCH 0x0000 // (INV_PITCH >> 1)
-#define INV_CTRL_ROLL 0x0001 // (INV_ROLL >> 1)
-#define INV_CTRL_YAW 0x0002 // (INV_YAW >> 1)
-
- /*******************************************************************************/
- /* control_params structure default values. */
- /*******************************************************************************/
-
-#define MLCTRL_SENSITIVITY_0_DEFAULT 128
-#define MLCTRL_SENSITIVITY_1_DEFAULT 128
-#define MLCTRL_SENSITIVITY_2_DEFAULT 128
-#define MLCTRL_SENSITIVITY_3_DEFAULT 128
-#define MLCTRL_FUNCTIONS_DEFAULT 0
-#define MLCTRL_CONTROL_SIGNALS_DEFAULT 0
-#define MLCTRL_PARAMETER_ARRAY_0_DEFAULT 0
-#define MLCTRL_PARAMETER_ARRAY_1_DEFAULT 0
-#define MLCTRL_PARAMETER_ARRAY_2_DEFAULT 0
-#define MLCTRL_PARAMETER_ARRAY_3_DEFAULT 0
-#define MLCTRL_PARAMETER_AXIS_0_DEFAULT 0
-#define MLCTRL_PARAMETER_AXIS_1_DEFAULT 0
-#define MLCTRL_PARAMETER_AXIS_2_DEFAULT 0
-#define MLCTRL_PARAMETER_AXIS_3_DEFAULT 0
-#define MLCTRL_GRID_THRESHOLD_0_DEFAULT 1
-#define MLCTRL_GRID_THRESHOLD_1_DEFAULT 1
-#define MLCTRL_GRID_THRESHOLD_2_DEFAULT 1
-#define MLCTRL_GRID_THRESHOLD_3_DEFAULT 1
-#define MLCTRL_GRID_MAXIMUM_0_DEFAULT 0
-#define MLCTRL_GRID_MAXIMUM_1_DEFAULT 0
-#define MLCTRL_GRID_MAXIMUM_2_DEFAULT 0
-#define MLCTRL_GRID_MAXIMUM_3_DEFAULT 0
-#define MLCTRL_GRID_CALLBACK_DEFAULT 0
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
- /**************************************************************************/
- /* Control Parameters Structure. */
- /**************************************************************************/
-
- struct control_params {
- // Sensitivity of control signal 1, 2, 3, and 4.
- unsigned short sensitivity[4];
- // Indicates what functions will be used. Can be a bitwise OR of INV_GRID,
- // ML_SMOOT, INV_DEAD_ZONE, and INV_HYSTERISIS.
- unsigned short functions;
- // Indicates which parameter array is being assigned to a control signal.
- // Must be one of INV_GYROS, INV_ANGULAR_VELOCITY, or
- // INV_ANGULAR_VELOCITY_WORLD.
- unsigned short parameterArray[4];
- // Indicates which axis of the parameter array will be used. Must be
- // INV_ROLL, INV_PITCH, or INV_YAW.
- unsigned short parameterAxis[4];
- // Threshold of the control signal at which the grid number will be
- // incremented or decremented.
- long gridThreshold[4];
- // Maximum grid number for the control signal.
- long gridMaximum[4];
- // User defined callback that will trigger when the grid location changes.
- void (*gridCallback) (
- // Indicates which control signal crossed a grid threshold. Must be
- // one of INV_CONTROL_1, INV_CONTROL_2, INV_CONTROL_3 or INV_CONTROL_4.
- unsigned short controlSignal,
- // An array of four numbers representing the grid number for each
- // control signal.
- long *gridNum,
- // An array of four numbers representing the change in grid number
- // for each control signal.
- long *gridChange);
- };
-
- struct control_obj {
-
- long gridNum[4]; // Current grid number for each control signal.
- long controlInt[4]; // Current data for each control signal.
- long lastGridNum[4]; // Previous grid number
- unsigned char controlDir[4]; // Direction of control signal
- long gridChange[4]; // Change in grid number
-
- long mlGridNumDMP[4];
- long gridNumOffset[4];
- long prevDMPGridNum[4];
-
- };
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-
- /**************************************************************************/
- /* 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
-}
-#endif
-#endif /* MLCONTROL_H */
diff --git a/60xx/mlsdk/mllite/mldl.c b/60xx/mlsdk/mllite/mldl.c
deleted file mode 100644
index 7054532..0000000
--- a/60xx/mlsdk/mllite/mldl.c
+++ /dev/null
@@ -1,1092 +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: mldl.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *****************************************************************************/
-
-/**
- * @defgroup MLDL
- * @brief Motion Library - Driver Layer.
- * The Motion Library Driver Layer provides the intrface to the
- * system drivers that are used by the Motion Library.
- *
- * @{
- * @file mldl.c
- * @brief The Motion Library Driver Layer.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <string.h>
-
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-# include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-# include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-# include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "compass.h"
-#include "mlsl.h"
-#include "mlos.h"
-#include "mlinclude.h"
-#include "ml.h"
-#include "dmpKey.h"
-#include "mlFIFOHW.h"
-#include "compass.h"
-#include "pressure.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mldl"
-
-#define _mldlDebug(x) //{x}
-
-/* --------------------- */
-/* - Variables. - */
-/* --------------------- */
-
-#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */
-
-/*---- structure containing control variables used by MLDL ----*/
-static struct mldl_cfg mldlCfg;
-struct ext_slave_descr gAccel;
-struct ext_slave_descr gCompass;
-struct ext_slave_descr gPressure;
-struct mpu_platform_data gPdata;
-static void *sMLSLHandle;
-int_fast8_t intTrigger[NUM_OF_INTSOURCES];
-
-/*******************************************************************************
- * Functions for accessing the DMP memory via keys
- ******************************************************************************/
-
-unsigned short (*sGetAddress) (unsigned short key) = NULL;
-static const unsigned char *localDmpMemory = NULL;
-static unsigned short localDmpMemorySize = 0;
-
-/**
- * @internal
- * @brief Sets the function to use to convert keys to addresses. This
- * will changed for each DMP code loaded.
- * @param func
- * Function used to convert keys to addresses.
- * @endif
- */
-void inv_set_get_address(unsigned short (*func) (unsigned short key))
-{
- INVENSENSE_FUNC_START;
- _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func);
- )
- sGetAddress = func;
-}
-
-/**
- * @internal
- * @brief Check if the feature is supported in the currently loaded
- * DMP code basing on the fact that the key is assigned a
- * value or not.
- * @param key the DMP key
- * @return whether the feature associated with the key is supported
- * or not.
- */
-uint_fast8_t inv_dmpkey_supported(unsigned short key)
-{
- unsigned short memAddr;
-
- if (sGetAddress == NULL) {
- MPL_LOGE("%s : sGetAddress is NULL\n", __func__);
- return FALSE;
- }
-
- memAddr = sGetAddress(key);
- if (memAddr >= 0xffff) {
- MPL_LOGV("inv_set_mpu_memory unsupported key\n");
- return FALSE;
- }
-
- return TRUE;
-}
-
-/**
- * @internal
- * @brief used to get the specified number of bytes from the original
- * MPU memory location specified by the key.
- * Reads the specified number of bytes from the MPU location
- * that was used to program the MPU specified by the key. Each
- * set of code specifies a function that changes keys into
- * addresses. This function is set with setGetAddress().
- *
- * @param key The key to use when looking up the address.
- * @param length Number of bytes to read.
- * @param buffer Result for data.
- *
- * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
- * not corresponding to a memory address will result in INV_ERROR.
- * @endif
- */
-inv_error_t inv_get_mpu_memory_original(unsigned short key,
- unsigned short length,
- unsigned char *buffer)
-{
- unsigned short offset;
-
- if (sGetAddress == NULL) {
- return INV_ERROR_NOT_OPENED;
- }
-
- offset = sGetAddress(key);
- if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- memcpy(buffer, &localDmpMemory[offset], length);
-
- return INV_SUCCESS;
-}
-
-unsigned short inv_dl_get_address(unsigned short key)
-{
- unsigned short offset;
- if (sGetAddress == NULL) {
- return INV_ERROR_NOT_OPENED;
- }
-
- offset = sGetAddress(key);
- return offset;
-}
-
-/* ---------------------- */
-/* - Static Functions. - */
-/* ---------------------- */
-
-/**
- * @brief Open the driver layer and resets the internal
- * gyroscope, accelerometer, and compass data
- * structures.
- * @param mlslHandle
- * the serial handle.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_dl_open(void *mlslHandle)
-{
- inv_error_t result;
- memset(&mldlCfg, 0, sizeof(mldlCfg));
- memset(intTrigger, INT_CLEAR, sizeof(intTrigger));
-
- sMLSLHandle = mlslHandle;
-
- mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */
- mldlCfg.accel = &gAccel;
- mldlCfg.compass = &gCompass;
- mldlCfg.pressure = &gPressure;
- mldlCfg.pdata = &gPdata;
-
- result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle,
- sMLSLHandle, sMLSLHandle, sMLSLHandle);
- return result;
-}
-
-/**
- * @brief Closes/Cleans up the ML Driver Layer.
- * Put the device in sleep mode.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_dl_close(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- result = (inv_error_t) inv_mpu_suspend(&mldlCfg,
- sMLSLHandle,
- sMLSLHandle,
- sMLSLHandle,
- sMLSLHandle,
- INV_ALL_SENSORS);
-
- result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle,
- sMLSLHandle, sMLSLHandle, sMLSLHandle);
- /* Clear all previous settings */
- memset(&mldlCfg, 0, sizeof(mldlCfg));
- sMLSLHandle = NULL;
- sGetAddress = NULL;
- return result;
-}
-
-/**
- * @brief Sets the requested_sensors
- *
- * Accessor to set the requested_sensors field of the mldl_cfg structure.
- * Typically set at initialization.
- *
- * @param sensors
- * Bitfield of the sensors that are going to be used. Combination of the
- * following:
- * - INV_X_GYRO
- * - INV_Y_GYRO
- * - INV_Z_GYRO
- * - INV_DMP_PROCESSOR
- * - INV_X_ACCEL
- * - INV_Y_ACCEL
- * - INV_Z_ACCEL
- * - INV_X_COMPASS
- * - INV_Y_COMPASS
- * - INV_Z_COMPASS
- * - INV_X_PRESSURE
- * - INV_Y_PRESSURE
- * - INV_Z_PRESSURE
- * - INV_THREE_AXIS_GYRO
- * - INV_THREE_AXIS_ACCEL
- * - INV_THREE_AXIS_COMPASS
- * - INV_THREE_AXIS_PRESSURE
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_init_requested_sensors(unsigned long sensors)
-{
- mldlCfg.requested_sensors = sensors;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Starts the DMP running
- *
- * Resumes the sensor if any of the sensor axis or components are requested
- *
- * @param sensors
- * Bitfield of the sensors to turn on. Combination of the following:
- * - INV_X_GYRO
- * - INV_Y_GYRO
- * - INV_Z_GYRO
- * - INV_DMP_PROCESSOR
- * - INV_X_ACCEL
- * - INV_Y_ACCEL
- * - INV_Z_ACCEL
- * - INV_X_COMPASS
- * - INV_Y_COMPASS
- * - INV_Z_COMPASS
- * - INV_X_PRESSURE
- * - INV_Y_PRESSURE
- * - INV_Z_PRESSURE
- * - INV_THREE_AXIS_GYRO
- * - INV_THREE_AXIS_ACCEL
- * - INV_THREE_AXIS_COMPASS
- * - INV_THREE_AXIS_PRESSURE
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_dl_start(unsigned long sensors)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- mldlCfg.requested_sensors = sensors;
- result = inv_mpu_resume(&mldlCfg,
- sMLSLHandle,
- sMLSLHandle,
- sMLSLHandle,
- sMLSLHandle,
- sensors);
- return result;
-}
-
-/**
- * @brief Stops the DMP running and puts it in low power as requested
- *
- * Suspends each sensor according to the bitfield, if all axis and components
- * of the sensor is off.
- *
- * @param sensors Bitfiled of the sensors to leave on. Combination of the
- * following:
- * - INV_X_GYRO
- * - INV_Y_GYRO
- * - INV_Z_GYRO
- * - INV_X_ACCEL
- * - INV_Y_ACCEL
- * - INV_Z_ACCEL
- * - INV_X_COMPASS
- * - INV_Y_COMPASS
- * - INV_Z_COMPASS
- * - INV_X_PRESSURE
- * - INV_Y_PRESSURE
- * - INV_Z_PRESSURE
- * - INV_THREE_AXIS_GYRO
- * - INV_THREE_AXIS_ACCEL
- * - INV_THREE_AXIS_COMPASS
- * - INV_THREE_AXIS_PRESSURE
- *
- *
- * @return INV_SUCCESS or non-zero error code
- */
-inv_error_t inv_dl_stop(unsigned long sensors)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result = INV_SUCCESS;
-
- result = inv_mpu_suspend(&mldlCfg,
- sMLSLHandle,
- sMLSLHandle,
- sMLSLHandle,
- sMLSLHandle,
- sensors);
- return result;
-}
-
-/**
- * @brief Get a pointer to the internal data structure
- * storing the configuration for the MPU, the accelerometer
- * and the compass in use.
- * @return a pointer to the data structure of type 'struct mldl_cfg'.
- */
-struct mldl_cfg *inv_get_dl_config(void)
-{
- return &mldlCfg;
-}
-
-/**
- * @brief Query the MPU slave address.
- * @return The 7-bit mpu slave address.
- */
-unsigned char inv_get_mpu_slave_addr(void)
-{
- INVENSENSE_FUNC_START;
- return mldlCfg.addr;
-}
-
-/**
- * @internal
- * @brief MLDLCfgDMP configures the Digital Motion Processor internal to
- * the MPU. The DMP can be enabled or disabled and the start address
- * can be set.
- *
- * @param enableRun Enables the DMP processing if set to TRUE.
- * @param enableFIFO Enables DMP output to the FIFO if set to TRUE.
- * @param startAddress start address
- *
- * @return Zero if the command is successful, an error code otherwise.
-*/
-inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
- unsigned char enableFIFO)
-{
- INVENSENSE_FUNC_START;
-
- mldlCfg.dmp_enable = enableRun;
- mldlCfg.fifo_enable = enableFIFO;
- mldlCfg.gyro_needs_reset = TRUE;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin.
- * The basic interrupt signal characteristics can be set
- * (i.e. active high/low, open drain/push pull, etc.) and the
- * triggers can be set.
- * Currently only INTPIN_MPU is supported.
- *
- * @param triggers
- * bitmask of triggers to enable for interrupt.
- * The available triggers are:
- * - BIT_MPU_RDY_EN
- * - BIT_DMP_INT_EN
- * - BIT_RAW_RDY_EN
- *
- * @return Zero if the command is successful, an error code otherwise.
-*/
-inv_error_t inv_get_dl_cfg_int(unsigned char triggers)
-{
- inv_error_t result = INV_SUCCESS;
-
-#if defined CONFIG_MPU_SENSORS_MPU3050
- /* Mantis has 8 bits of interrupt config bits */
- if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-#endif
-
- mldlCfg.int_config = triggers;
- if (!mldlCfg.gyro_is_suspended) {
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_INT_CFG,
- (mldlCfg.int_config | mldlCfg.pdata->
- int_config));
- } else {
- mldlCfg.gyro_needs_reset = TRUE;
- }
-
- return result;
-}
-
-/**
- * @brief configures the output sampling rate on the MPU.
- * Three parameters control the sampling:
- *
- * 1) Low pass filter bandwidth, and
- * 2) output sampling divider.
- *
- * The output sampling rate is determined by the divider and the low
- * pass filter setting. If the low pass filter is set to
- * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the
- * divider is 8kHz; for all other settings it is 1kHz.
- * The 8-bit divider will divide this frequency to get the resulting
- * sample frequency.
- * For example, if the filter setting is not 256Hz and the divider is
- * set to 7, then the sample rate is as follows:
- * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms).
- *
- * The low pass filter selection codes control both the cutoff frequency of
- * the internal low pass filter and internal analog sampling rate. The
- * latter, in turn, affects the final output sampling rate according to the
- * sample rate divider settig.
- * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate,
- * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate,
- * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate,
- * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate,
- * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate,
- * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate,
- * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate,
- * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate.
- *
- * @param lpf low pass filter, 0 to 7.
- * @param divider Output sampling rate divider, 0 to 255.
- *
- * @return ML_SUCESS if successful; a non-zero error code otherwise.
-**/
-inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider)
-{
- /*---- do range checking ----*/
- if (lpf >= NUM_MPU_FILTER) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- mldlCfg.lpf = lpf;
- mldlCfg.divider = divider;
- mldlCfg.gyro_needs_reset = TRUE;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief set the full scale range for the gyros.
- * The full scale selection codes correspond to:
- * 0 -> 250 dps,
- * 1 -> 500 dps,
- * 2 -> 1000 dps,
- * 3 -> 2000 dps.
- * Full scale range affect the MPU's measurement
- * sensitivity.
- *
- * @param fullScale
- * the gyro full scale range in dps.
- *
- * @return INV_SUCCESS or non-zero error code.
-**/
-inv_error_t inv_set_full_scale(float fullScale)
-{
- if (fullScale == 250.f)
- mldlCfg.full_scale = MPU_FS_250DPS;
- else if (fullScale == 500.f)
- mldlCfg.full_scale = MPU_FS_500DPS;
- else if (fullScale == 1000.f)
- mldlCfg.full_scale = MPU_FS_1000DPS;
- else if (fullScale == 2000.f)
- mldlCfg.full_scale = MPU_FS_2000DPS;
- else { // not a valid setting
- MPL_LOGE("Invalid full scale range specification for gyros : %f\n",
- fullScale);
- MPL_LOGE
- ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n");
- return INV_ERROR_INVALID_PARAMETER;
- }
- mldlCfg.gyro_needs_reset = TRUE;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief This function sets the external sync for the MPU sampling.
- * It can be synchronized on the LSB of any of the gyros, any of the
- * external accels, or on the temp readings.
- *
- * @param extSync External sync selection, 0 to 7.
- * @return Zero if the command is successful; an error code otherwise.
-**/
-inv_error_t inv_set_external_sync(unsigned char extSync)
-{
- INVENSENSE_FUNC_START;
-
- /*---- do range checking ----*/
- if (extSync >= NUM_MPU_EXT_SYNC) {
- return INV_ERROR_INVALID_PARAMETER;
- }
- mldlCfg.ext_sync = extSync;
- mldlCfg.gyro_needs_reset = TRUE;
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_set_ignore_system_suspend(unsigned char ignore)
-{
- INVENSENSE_FUNC_START;
-
- mldlCfg.ignore_system_suspend = ignore;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief inv_clock_source function sets the clock source for the MPU gyro
- * processing.
- * The source can be any of the following:
- * - Internal 8MHz oscillator,
- * - PLL with X gyro as reference,
- * - PLL with Y gyro as reference,
- * - PLL with Z gyro as reference,
- * - PLL with external 32.768Mhz reference, or
- * - PLL with external 19.2MHz reference
- *
- * For best accuracy and timing, it is highly recommended to use one
- * of the gyros as the clock source; however this gyro must be
- * enabled to use its clock (see 'MLDLPowerMgmtMPU()').
- *
- * @param clkSource Clock source selection.
- * Can be one of:
- * - CLK_INTERNAL,
- * - CLK_PLLGYROX,
- * - CLK_PLLGYROY,
- * - CLK_PLLGYROZ,
- * - CLK_PLLEXT32K, or
- * - CLK_PLLEXT19M.
- *
- * @return Zero if the command is successful; an error code otherwise.
-**/
-inv_error_t inv_clock_source(unsigned char clkSource)
-{
- INVENSENSE_FUNC_START;
-
- /*---- do range checking ----*/
- if (clkSource >= NUM_CLK_SEL) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- mldlCfg.clk_src = clkSource;
- mldlCfg.gyro_needs_reset = TRUE;
-
- return INV_SUCCESS;
-}
-
-/**
- * @brief Set the Temperature Compensation offset.
- * @param tc
- * a pointer to the temperature compensations offset
- * for the 3 gyro axes.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_set_offsetTC(const unsigned char *tc)
-{
- int ii;
- inv_error_t result;
-
- for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) {
- mldlCfg.offset_tc[ii] = tc[ii];
- }
-
- if (!mldlCfg.gyro_is_suspended) {
-#ifdef CONFIG_MPU_SENSORS_MPU3050
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_XG_OFFS_TC, tc[0]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_YG_OFFS_TC, tc[1]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_ZG_OFFS_TC, tc[2]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#else
- unsigned char reg;
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_XG_OFFS_TC,
- ((mldlCfg.offset_tc[0] << 1)
- & BITS_XG_OFFS_TC));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC);
-#ifdef CONFIG_MPU_SENSORS_MPU6050B1
- if (mldlCfg.pdata->level_shifter)
- reg |= BIT_I2C_MST_VDDIO;
-#endif
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_YG_OFFS_TC, reg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr,
- MPUREG_ZG_OFFS_TC,
- ((mldlCfg.offset_tc[2] << 1)
- & BITS_ZG_OFFS_TC));
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#endif
- } else {
- mldlCfg.gyro_needs_reset = TRUE;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief Set the gyro offset.
- * @param offset
- * a pointer to the gyro offset for the 3 gyro axes. This is scaled
- * as it would be written to the hardware registers.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_set_offset(const short *offset)
-{
- inv_error_t result;
- unsigned char regs[7];
- int ii;
- long sf;
-
- sf = (2000L * 131) / mldlCfg.gyro_sens_trim;
- for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) {
- // Record the bias in the units the register uses
- mldlCfg.offset[ii] = offset[ii];
- // Convert the bias to 1 dps = 1<<16
- inv_obj.gyro_bias[ii] = -offset[ii] * sf;
- regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff;
- regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff);
- }
-
- if (!mldlCfg.gyro_is_suspended) {
- regs[0] = MPUREG_X_OFFS_USRH;
- result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else {
- mldlCfg.gyro_needs_reset = TRUE;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @internal
- * @brief used to get the specified number of bytes in the specified MPU
- * memory bank.
- * The memory bank is one of the following:
- * - MPUMEM_RAM_BANK_0,
- * - MPUMEM_RAM_BANK_1,
- * - MPUMEM_RAM_BANK_2, or
- * - MPUMEM_RAM_BANK_3.
- *
- * @param bank Memory bank to write.
- * @param memAddr Starting address for write.
- * @param length Number of bytes to write.
- * @param buffer Result for data.
- *
- * @return zero if the command is successful, an error code otherwise.
- * @endif
- */
-inv_error_t
-inv_get_mpu_memory_one_bank(unsigned char bank,
- unsigned char memAddr,
- unsigned short length, unsigned char *buffer)
-{
- inv_error_t result;
-
- if ((bank >= MPU_MEM_NUM_RAM_BANKS) ||
- //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255
- ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- if (mldlCfg.gyro_is_suspended) {
- memcpy(buffer, &mldlCfg.ram[bank][memAddr], length);
- result = INV_SUCCESS;
- } else {
- result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr,
- ((bank << 8) | memAddr), length, buffer);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
- return result;
-}
-
-/**
- * @internal
- * @brief used to set the specified number of bytes in the specified MPU
- * memory bank.
- * The memory bank is one of the following:
- * - MPUMEM_RAM_BANK_0,
- * - MPUMEM_RAM_BANK_1,
- * - MPUMEM_RAM_BANK_2, or
- * - MPUMEM_RAM_BANK_3.
- *
- * @param bank Memory bank to write.
- * @param memAddr Starting address for write.
- * @param length Number of bytes to write.
- * @param buffer Result for data.
- *
- * @return zero if the command is successful, an error code otherwise.
- * @endif
- */
-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 result = INV_SUCCESS;
- int different;
-
- if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) ||
- ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length);
- memcpy(&mldlCfg.ram[bank][memAddr], buffer, length);
- if (!mldlCfg.gyro_is_suspended) {
- result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr,
- ((bank << 8) | memAddr), length, buffer);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else if (different) {
- mldlCfg.gyro_needs_reset = TRUE;
- }
-
- return result;
-}
-
-/**
- * @internal
- * @brief used to get the specified number of bytes from the MPU location
- * specified by the key.
- * Reads the specified number of bytes from the MPU location
- * specified by the key. Each set of code specifies a function
- * that changes keys into addresses. This function is set with
- * setGetAddress().
- *
- * @param key The key to use when looking up the address.
- * @param length Number of bytes to read.
- * @param buffer Result for data.
- *
- * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
- * not corresponding to a memory address will result in INV_ERROR.
- * @endif
- */
-inv_error_t inv_get_mpu_memory(unsigned short key,
- unsigned short length, unsigned char *buffer)
-{
- unsigned char bank;
- inv_error_t result;
- unsigned short memAddr;
-
- if (sGetAddress == NULL) {
- return INV_ERROR_NOT_OPENED;
- }
-
- memAddr = sGetAddress(key);
- if (memAddr >= 0xffff)
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
- bank = memAddr >> 8; // Get Bank
- memAddr &= 0xff;
-
- while (memAddr + length > MPU_MEM_BANK_SIZE) {
- // We cross a bank in the middle
- unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
- result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
- sub_length, buffer);
- if (INV_SUCCESS != result)
- return result;
- bank++;
- length -= sub_length;
- buffer += sub_length;
- memAddr = 0;
- }
- result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr,
- length, buffer);
-
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-/**
- * @internal
- * @brief used to set the specified number of bytes from the MPU location
- * specified by the key.
- * Set the specified number of bytes from the MPU location
- * specified by the key. Each set of DMP code specifies a function
- * that changes keys into addresses. This function is set with
- * setGetAddress().
- *
- * @param key The key to use when looking up the address.
- * @param length Number of bytes to write.
- * @param buffer Result for data.
- *
- * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key
- * not corresponding to a memory address will result in INV_ERROR.
- * @endif
- */
-inv_error_t inv_set_mpu_memory(unsigned short key,
- unsigned short length,
- const unsigned char *buffer)
-{
- inv_error_t result = INV_SUCCESS;
- unsigned short memAddr;
- unsigned char bank;
-
- if (sGetAddress == NULL) {
- MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n");
- return INV_ERROR_INVALID_MODULE;
- }
- memAddr = sGetAddress(key);
-
- if (memAddr >= 0xffff) {
- MPL_LOGE("inv_set_mpu_memory unsupported key\n");
- return INV_ERROR_INVALID_MODULE; // This key not supported
- }
-
- bank = (unsigned char)(memAddr >> 8);
- memAddr &= 0xff;
-
- while (memAddr + length > MPU_MEM_BANK_SIZE) {
- // We cross a bank in the middle
- unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr;
-
- result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- bank++;
- length -= sub_length;
- buffer += sub_length;
- memAddr = 0;
- }
- result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return result;
-}
-
-/**
- * @brief Load the DMP with the given code and configuration.
- * @param buffer
- * the DMP data.
- * @param length
- * the length in bytes of the DMP data.
- * @param config
- * the DMP configuration.
- * @return INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_load_dmp(const unsigned char *buffer,
- unsigned short length, unsigned short config)
-{
- INVENSENSE_FUNC_START;
-
- inv_error_t result = INV_SUCCESS;
- unsigned short toWrite;
- unsigned short memAddr = 0;
- localDmpMemory = buffer;
- localDmpMemorySize = length;
-
- mldlCfg.dmp_cfg1 = (config >> 8);
- mldlCfg.dmp_cfg2 = (config & 0xff);
-
- while (length > 0) {
- toWrite = length;
- if (toWrite > MAX_LOAD_WRITE_SIZE)
- toWrite = MAX_LOAD_WRITE_SIZE;
-
- result =
- inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite,
- buffer);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- buffer += toWrite;
- memAddr += toWrite;
- length -= toWrite;
- }
-
- return result;
-}
-
-/**
- * @brief Get the silicon revision ID.
- * @return The silicon revision ID
- * (0 will be read if inv_mpu_open returned an error)
- */
-unsigned char inv_get_silicon_rev(void)
-{
- return mldlCfg.silicon_revision;
-}
-
-/**
- * @brief Get the product revision ID.
- * @return The product revision ID
- * (0 will be read if inv_mpu_open returned an error)
- */
-unsigned char inv_get_product_rev(void)
-{
- return mldlCfg.product_revision;
-}
-
-/*******************************************************************************
- *******************************************************************************
- *******************************************************************************
- * @todo these belong with an interface to the kernel driver layer
- *******************************************************************************
- *******************************************************************************
- ******************************************************************************/
-
-/**
- * @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.
- * Currently the only source supported is INTPIN_MPU.
- *
- * @return 1 if the interrupt has been triggered.
- */
-unsigned char inv_get_interrupt_trigger(unsigned char srcIndex)
-{
- INVENSENSE_FUNC_START;
- return intTrigger[srcIndex];
-}
-
-/**
- * @brief clear the 'triggered' status for an interrupt source.
- * @param srcIndex
- * index of the interrupt source.
- * Currently only INTPIN_MPU is supported.
- */
-void inv_clear_interrupt_trigger(unsigned char srcIndex)
-{
- INVENSENSE_FUNC_START;
- 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/60xx/mlsdk/mllite/mldl.h b/60xx/mlsdk/mllite/mldl.h
deleted file mode 100644
index 961d860..0000000
--- a/60xx/mlsdk/mllite/mldl.h
+++ /dev/null
@@ -1,176 +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: mldl.h 5639 2011-06-14 01:23:05Z nroyer $
- *
- *******************************************************************************/
-
-#ifndef MLDL_H
-#define MLDL_H
-
-#include "mltypes.h"
-#include "mlsl.h"
-#include <linux/mpu.h>
-#include "mldl_cfg.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mldl_legacy.h"
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
-typedef enum _DEVICE_CONFIG {
- DEVICE_MPU_ACCEL = 0,
- DEVICE_MPU,
- NUM_OF_DEVICES
-} DEVICE_CONFIG;
-
-#define SERIAL_I2C 0
-#define SERIAL_SPI 1
-
-#define DATAMODE_MANUAL 0 // Manual data mode
-#define DATAMODE_AUTO 1 // Auto data mode
-
-#define DATASRC_IMMEDIATE 0 // Return data immediately
-#define DATASRC_WHENREADY 1 // Only return data when new data is available
-#define DATASRC_FIFO 2 // Use FIFO for data
-
-#define SENSOR_DATA_GYROX 0x0001
-#define SENSOR_DATA_GYROY 0x0002
-#define SENSOR_DATA_GYROZ 0x0004
-#define SENSOR_DATA_ACCELX 0x0008
-#define SENSOR_DATA_ACCELY 0x0010
-#define SENSOR_DATA_ACCELZ 0x0020
-#define SENSOR_DATA_AUX1 0x0040
-#define SENSOR_DATA_AUX2 0x0080
-#define SENSOR_DATA_AUX3 0x0100
-#define SENSOR_DATA_TEMP 0x0200
-
-#define INTPIN_MPU 0
-
-#define INTLOGIC_HIGH 0
-#define INTLOGIC_LOW 1
-
-#define INTTYPE_PUSHPULL 0
-#define INTTYPE_OPENDRAIN 1
-
-#define INTLATCH_DISABLE 0
-#define INTLATCH_ENABLE 1
-
-#define MPUINT_MPU_READY 0x04
-#define MPUINT_DMP_DONE 0x02
-#define MPUINT_DATA_READY 0x01
-
-#define INTLATCHCLEAR_READSTATUS 0
-#define INTLATCHCLEAR_ANYREAD 1
-
-#define DMP_DONTRUN 0
-#define DMP_RUN 1
-
- /*---- defines for external interrupt status (via external call into library) ----*/
-#define INT_CLEAR 0
-#define INT_TRIGGERED 1
-
-typedef enum {
- INTSRC_MPU = 0,
- INTSRC_AUX1,
- INTSRC_AUX2,
- INTSRC_AUX3,
- INTSRC_TIMER,
- INTSRC_UNKNOWN,
- INTSRC_MPU_FIFO,
- INTSRC_MPU_MOTION,
- NUM_OF_INTSOURCES,
-} INT_SOURCE;
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
- /* --------------- */
- /* - Variables. - */
- /* --------------- */
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_dl_open(void *mlslHandle);
- inv_error_t inv_dl_close(void);
-
- inv_error_t inv_dl_start(unsigned long sensors);
- inv_error_t inv_dl_stop(unsigned long sensors);
-
- struct mldl_cfg *inv_get_dl_config(void);
-
- inv_error_t inv_init_requested_sensors(unsigned long sensors);
- unsigned char inv_get_mpu_slave_addr(void);
- inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun,
- unsigned char enableFIFO);
- inv_error_t inv_get_dl_cfg_int(unsigned char triggers);
- inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider);
- inv_error_t inv_set_full_scale(float fullScale);
- inv_error_t inv_set_external_sync(unsigned char extSync);
- inv_error_t inv_set_ignore_system_suspend(unsigned char ignore);
- inv_error_t inv_clock_source(unsigned char clkSource);
- inv_error_t inv_get_mpu_memory(unsigned short key,
- unsigned short length,
- unsigned char *buffer);
- inv_error_t inv_set_mpu_memory(unsigned short key,
- unsigned short length,
- const unsigned char *buffer);
- inv_error_t inv_load_dmp(const unsigned char *buffer,
- unsigned short length,
- unsigned short startAddress);
-
- unsigned char inv_get_slicon_rev(void);
- inv_error_t inv_set_offsetTC(const unsigned char *tc);
- inv_error_t inv_set_offset(const short *offset);
-
- /* Functions for setting and retrieving the DMP memory */
- inv_error_t inv_get_mpu_memory_original(unsigned short key,
- unsigned short length,
- unsigned char *buffer);
- void inv_set_get_address(unsigned short (*func) (unsigned short key));
- 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
-#endif // MLDL_H
diff --git a/60xx/mlsdk/mllite/mldl_cfg.h b/60xx/mlsdk/mllite/mldl_cfg.h
deleted file mode 100644
index b4914e2..0000000
--- a/60xx/mlsdk/mllite/mldl_cfg.h
+++ /dev/null
@@ -1,336 +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.
- $
- */
-
-/**
- * @addtogroup MLDL
- *
- * @{
- * @file mldl_cfg.h
- * @brief The Motion Library Driver Layer Configuration header file.
- */
-
-#ifndef __MLDL_CFG_H__
-#define __MLDL_CFG_H__
-
-#include "mltypes.h"
-#include "mlsl.h"
-#include <linux/mpu.h>
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-# include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-# include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-# include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-
-#include "log.h"
-
-/*************************************************************************
- * Sensors
- *************************************************************************/
-
-#define INV_X_GYRO (0x0001)
-#define INV_Y_GYRO (0x0002)
-#define INV_Z_GYRO (0x0004)
-#define INV_DMP_PROCESSOR (0x0008)
-
-#define INV_X_ACCEL (0x0010)
-#define INV_Y_ACCEL (0x0020)
-#define INV_Z_ACCEL (0x0040)
-
-#define INV_X_COMPASS (0x0080)
-#define INV_Y_COMPASS (0x0100)
-#define INV_Z_COMPASS (0x0200)
-
-#define INV_X_PRESSURE (0x0300)
-#define INV_Y_PRESSURE (0x0800)
-#define INV_Z_PRESSURE (0x1000)
-
-#define INV_TEMPERATURE (0x2000)
-#define INV_TIME (0x4000)
-
-#define INV_THREE_AXIS_GYRO (0x000F)
-#define INV_THREE_AXIS_ACCEL (0x0070)
-#define INV_THREE_AXIS_COMPASS (0x0380)
-#define INV_THREE_AXIS_PRESSURE (0x1C00)
-
-#define INV_FIVE_AXIS (0x007B)
-#define INV_SIX_AXIS_GYRO_ACCEL (0x007F)
-#define INV_SIX_AXIS_ACCEL_COMPASS (0x03F0)
-#define INV_NINE_AXIS (0x03FF)
-#define INV_ALL_SENSORS (0x7FFF)
-
-#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev)
-
-/* -------------------------------------------------------------------------- */
-
-/* Platform data for the MPU */
-struct mldl_cfg {
- /* MPU related configuration */
- unsigned long requested_sensors;
- unsigned char ignore_system_suspend;
- unsigned char addr;
- unsigned char int_config;
- unsigned char ext_sync;
- unsigned char full_scale;
- unsigned char lpf;
- unsigned char clk_src;
- unsigned char divider;
- unsigned char dmp_enable;
- unsigned char fifo_enable;
- unsigned char dmp_cfg1;
- unsigned char dmp_cfg2;
- unsigned char offset_tc[GYRO_NUM_AXES];
- unsigned short offset[GYRO_NUM_AXES];
- unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE];
-
- /* MPU Related stored status and info */
- unsigned char product_revision;
- unsigned char silicon_revision;
- unsigned char product_id;
- unsigned short gyro_sens_trim;
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- unsigned short accel_sens_trim;
-#endif
-
- /* Driver/Kernel related state information */
- int gyro_is_bypassed;
- int i2c_slaves_enabled;
- int dmp_is_running;
- int gyro_is_suspended;
- int accel_is_suspended;
- int compass_is_suspended;
- int pressure_is_suspended;
- int gyro_needs_reset;
-
- /* Slave related information */
- struct ext_slave_descr *accel;
- struct ext_slave_descr *compass;
- struct ext_slave_descr *pressure;
-
- /* Platform Data */
- struct mpu_platform_data *pdata;
-};
-
-/* -------------------------------------------------------------------------- */
-
-int inv_mpu_open(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle);
-int inv_mpu_close(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle);
-int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle,
- unsigned long sensors);
-int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle,
- unsigned long sensors);
-
-/* Slave Read functions */
-int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *slave_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata,
- unsigned char *data);
-static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *accel_handle, unsigned char *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_slave_read(mldl_cfg, gyro_handle, accel_handle,
- mldl_cfg->accel, &mldl_cfg->pdata->accel,
- data);
-}
-
-static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *compass_handle,
- unsigned char *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_slave_read(mldl_cfg, gyro_handle, compass_handle,
- mldl_cfg->compass, &mldl_cfg->pdata->compass,
- data);
-}
-
-static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *pressure_handle,
- unsigned char *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_slave_read(mldl_cfg, gyro_handle, pressure_handle,
- mldl_cfg->pressure,
- &mldl_cfg->pdata->pressure, data);
-}
-
-/* Slave Config functions */
-int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *slave_handle,
- struct ext_slave_config *data,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata);
-static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *accel_handle,
- struct ext_slave_config *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_slave_config(mldl_cfg, gyro_handle, accel_handle, data,
- mldl_cfg->accel, &mldl_cfg->pdata->accel);
-}
-
-static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *compass_handle,
- struct ext_slave_config *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_slave_config(mldl_cfg, gyro_handle, compass_handle, data,
- mldl_cfg->compass,
- &mldl_cfg->pdata->compass);
-}
-
-static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *pressure_handle,
- struct ext_slave_config *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_slave_config(mldl_cfg, gyro_handle, pressure_handle,
- data, mldl_cfg->pressure,
- &mldl_cfg->pdata->pressure);
-}
-
-/* Slave get config functions */
-int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *slave_handle,
- struct ext_slave_config *data,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata);
-
-static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *accel_handle,
- struct ext_slave_config *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, accel_handle,
- data, mldl_cfg->accel,
- &mldl_cfg->pdata->accel);
-}
-
-static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *compass_handle,
- struct ext_slave_config *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, compass_handle,
- data, mldl_cfg->compass,
- &mldl_cfg->pdata->compass);
-}
-
-static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *pressure_handle,
- struct ext_slave_config *data)
-{
- if (!mldl_cfg || !(mldl_cfg->pdata)) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- return inv_mpu_get_slave_config(mldl_cfg, gyro_handle,
- pressure_handle, data,
- mldl_cfg->pressure,
- &mldl_cfg->pdata->pressure);
-}
-
-/* -------------------------------------------------------------------------- */
-
-static inline long inv_mpu_get_sampling_rate_hz(struct mldl_cfg *mldl_cfg)
-{
- if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7))
- return 8000L / (mldl_cfg->divider + 1);
- else
- return 1000L / (mldl_cfg->divider + 1);
-}
-
-static inline long inv_mpu_get_sampling_period_us(struct mldl_cfg *mldl_cfg)
-{
- if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7))
- return (long) (1000000L * (mldl_cfg->divider + 1)) / 8000L;
- else
- return (long) (1000000L * (mldl_cfg->divider + 1)) / 1000L;
-}
-
-#endif /* __MLDL_CFG_H__ */
-
-/**
- *@}
- */
diff --git a/60xx/mlsdk/mllite/mldl_cfg_mpu.c b/60xx/mlsdk/mllite/mldl_cfg_mpu.c
deleted file mode 100644
index 3d4496e..0000000
--- a/60xx/mlsdk/mllite/mldl_cfg_mpu.c
+++ /dev/null
@@ -1,477 +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: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
- *
- *****************************************************************************/
-
-/**
- * @addtogroup MLDL
- *
- * @{
- * @file mldl_cfg_mpu.c
- * @brief The Motion Library Driver Layer.
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <stddef.h>
-#include "mldl_cfg.h"
-#include "mlsl.h"
-#include "mpu.h"
-
-#ifdef LINUX
-#include <sys/ioctl.h>
-#endif
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mldl_cfg_mpu:"
-
-/* --------------------- */
-/* - Variables. - */
-/* --------------------- */
-
-
-/* ---------------------- */
-/* - Static Functions. - */
-/* ---------------------- */
-void mpu_print_cfg(struct mldl_cfg * mldl_cfg)
-{
- struct mpu_platform_data *pdata = mldl_cfg->pdata;
- struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel;
- struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass;
- struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure;
-
- MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr);
- MPL_LOGD("mldl_cfg.int_config = %02x\n", mldl_cfg->int_config);
- MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync);
- MPL_LOGD("mldl_cfg.full_scale = %02x\n", mldl_cfg->full_scale);
- MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf);
- MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src);
- MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider);
- MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", mldl_cfg->dmp_enable);
- MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", mldl_cfg->fifo_enable);
- MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1);
- MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2);
- MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", mldl_cfg->offset_tc[0]);
- MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", mldl_cfg->offset_tc[1]);
- MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", mldl_cfg->offset_tc[2]);
- MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision);
- MPL_LOGD("mldl_cfg.product_id = %02x\n", mldl_cfg->product_id);
- MPL_LOGD("mldl_cfg.gyro_sens_trim = %02x\n", mldl_cfg->gyro_sens_trim);
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1
- MPL_LOGD("mldl_cfg.accel_sens_trim = %02x\n", mldl_cfg->accel_sens_trim);
-#endif
-
- if (mldl_cfg->accel) {
- MPL_LOGD("slave_accel->suspend = %p\n", mldl_cfg->accel->suspend);
- MPL_LOGD("slave_accel->resume = %p\n", mldl_cfg->accel->resume);
- MPL_LOGD("slave_accel->read = %p\n", mldl_cfg->accel->read);
- MPL_LOGD("slave_accel->type = %02x\n", mldl_cfg->accel->type);
- MPL_LOGD("slave_accel->read_reg = %02x\n",
- mldl_cfg->accel->read_reg);
- MPL_LOGD("slave_accel->read_len = %02x\n",
- mldl_cfg->accel->read_len);
- MPL_LOGD("slave_accel->endian = %02x\n", mldl_cfg->accel->endian);
- MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa);
- MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction);
- } else {
- MPL_LOGD("slave_accel = NULL\n");
- }
-
- if (mldl_cfg->compass) {
- MPL_LOGD("slave_compass->suspend = %p\n", mldl_cfg->compass->suspend);
- MPL_LOGD("slave_compass->resume = %p\n", mldl_cfg->compass->resume);
- MPL_LOGD("slave_compass->read = %p\n", mldl_cfg->compass->read);
- MPL_LOGD("slave_compass->type = %02x\n", mldl_cfg->compass->type);
- MPL_LOGD("slave_compass->read_reg = %02x\n",
- mldl_cfg->compass->read_reg);
- MPL_LOGD("slave_compass->read_len = %02x\n",
- mldl_cfg->compass->read_len);
- MPL_LOGD("slave_compass->endian = %02x\n", mldl_cfg->compass->endian);
- MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa);
- MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction);
- } else {
- MPL_LOGD("slave_compass = NULL\n");
- }
-
- if (mldl_cfg->pressure) {
- MPL_LOGD("slave_pressure->suspend = %p\n", mldl_cfg->pressure->suspend);
- MPL_LOGD("slave_pressure->resume = %p\n", mldl_cfg->pressure->resume);
- MPL_LOGD("slave_pressure->read = %p\n", mldl_cfg->pressure->read);
- MPL_LOGD("slave_pressure->type = %02x\n", mldl_cfg->pressure->type);
- MPL_LOGD("slave_pressure->read_reg = %02x\n",
- mldl_cfg->pressure->read_reg);
- MPL_LOGD("slave_pressure->read_len = %02x\n",
- mldl_cfg->pressure->read_len);
- MPL_LOGD("slave_pressure->endian = %02x\n", mldl_cfg->pressure->endian);
- MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa);
- MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction);
- } else {
- MPL_LOGD("slave_pressure = NULL\n");
- }
-
- MPL_LOGD("accel->get_slave_descr = %p\n",accel->get_slave_descr);
- MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num);
- MPL_LOGD("accel->bus = %02x\n", accel->bus);
- MPL_LOGD("accel->address = %02x\n", accel->address);
- MPL_LOGD("accel->orientation = \n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n",
- accel->orientation[0],accel->orientation[1],accel->orientation[2],
- accel->orientation[3],accel->orientation[4],accel->orientation[5],
- accel->orientation[6],accel->orientation[7],accel->orientation[8]);
- MPL_LOGD("compass->get_slave_descr = %p\n",compass->get_slave_descr);
- MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num);
- MPL_LOGD("compass->bus = %02x\n", compass->bus);
- MPL_LOGD("compass->address = %02x\n", compass->address);
- MPL_LOGD("compass->orientation = \n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n",
- compass->orientation[0],compass->orientation[1],compass->orientation[2],
- compass->orientation[3],compass->orientation[4],compass->orientation[5],
- compass->orientation[6],compass->orientation[7],compass->orientation[8]);
- MPL_LOGD("pressure->get_slave_descr = %p\n",pressure->get_slave_descr);
- MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num);
- MPL_LOGD("pressure->bus = %02x\n", pressure->bus);
- MPL_LOGD("pressure->address = %02x\n", pressure->address);
- MPL_LOGD("pressure->orientation = \n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n",
- pressure->orientation[0],pressure->orientation[1],pressure->orientation[2],
- pressure->orientation[3],pressure->orientation[4],pressure->orientation[5],
- pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]);
-
- MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config);
- MPL_LOGD("pdata->level_shifter = %02x\n", pdata->level_shifter);
- MPL_LOGD("pdata->orientation = \n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n"
- " %2d %2d %2d\n",
- pdata->orientation[0],pdata->orientation[1],pdata->orientation[2],
- pdata->orientation[3],pdata->orientation[4],pdata->orientation[5],
- pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]);
-
- MPL_LOGD("Struct sizes: mldl_cfg: %zu, "
- "ext_slave_descr:%zu, mpu_platform_data:%zu: RamOffset: %zu\n",
- sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr),
- sizeof(struct mpu_platform_data),
- offsetof(struct mldl_cfg, ram));
-}
-
-/******************************************************************************
- ******************************************************************************
- * Exported functions
- ******************************************************************************
- *****************************************************************************/
-
-/**
- * Initializes the pdata structure to defaults.
- *
- * Opens the device to read silicon revision, product id and whoami. Leaves
- * the device in suspended state for low power.
- *
- * @param mldl_cfg handle to the config structure
- * @param mlsl_handle handle to the mpu serial layer
- * @param accel_handle handle to the accel serial layer
- * @param compass_handle handle to the compass serial layer
- * @param pressure_handle handle to the pressure serial layer
- *
- * @return INV_SUCCESS if silicon revision, product id and woami are supported
- * by this software.
- */
-int inv_mpu_open(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle)
-{
- int result;
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
- INV_ALL_SENSORS);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- return result;
-}
-
-/**
- * Stub for driver close. Just verify that the devices are suspended
- *
- * @param mldl_cfg handle to the config structure
- * @param mlsl_handle handle to the mpu serial layer
- * @param accel_handle handle to the accel serial layer
- * @param compass_handle handle to the compass serial layer
- * @param pressure_handle handle to the compass serial layer
- *
- * @return INV_SUCCESS or non-zero error code
- */
-int inv_mpu_close(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle)
-{
- int result = INV_SUCCESS;
-
- result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL,
- INV_ALL_SENSORS);
- return result;
-}
-
-int inv_mpu_resume(struct mldl_cfg* mldl_cfg,
- void *mlsl_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle,
- unsigned long sensors)
-{
- int result;
-
- mldl_cfg->requested_sensors = sensors;
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_RESUME, NULL);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- //MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors);
-
- return result;
-}
-
-
-int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
- void *accel_handle,
- void *compass_handle,
- void *pressure_handle,
- unsigned long sensors)
-{
- int result;
- unsigned long requested = mldl_cfg->requested_sensors;
-
- mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
- //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__,
- // mldl_cfg->requested_sensors);
-
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SUSPEND, NULL);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- mldl_cfg->requested_sensors = requested;
- //MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested);
-
- return result;
-}
-
-/**
- * Send slave configuration information
- *
- * @param mldl_cfg pointer to the mldl configuration structure
- * @param gyro_handle handle to the gyro sensor
- * @param slave_handle handle to the slave sensor
- * @param slave slave description
- * @param pdata slave platform data
- * @param data where to store the read data
- *
- * @return 0 or non-zero error code
- */
-int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *slave_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata,
- unsigned char *data)
-{
- int result;
- if (!mldl_cfg || !gyro_handle || !data || !slave) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- switch (slave->type) {
- case EXT_SLAVE_TYPE_ACCELEROMETER:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_ACCEL, data);
- break;
- case EXT_SLAVE_TYPE_COMPASS:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_COMPASS, data);
- break;
- case EXT_SLAVE_TYPE_PRESSURE:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_PRESSURE, data);
- break;
- default:
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- break;
- }
-
- return result;
-}
-
-/**
- * Send slave configuration information
- *
- * @param mldl_cfg pointer to the mldl configuration structure
- * @param gyro_handle handle to the gyro sensor
- * @param slave_handle handle to the slave sensor
- * @param data the data being sent
- * @param slave slave description
- * @param pdata slave platform data
- *
- * @return 0 or non-zero error code
- */
-int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *slave_handle,
- struct ext_slave_config *data,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata)
-{
- int result;
- if (!mldl_cfg || !data || !slave || !pdata || !slave) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- switch (slave->type) {
- case EXT_SLAVE_TYPE_ACCELEROMETER:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_ACCEL, data);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- break;
- case EXT_SLAVE_TYPE_COMPASS:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_COMPASS, data);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- break;
- case EXT_SLAVE_TYPE_PRESSURE:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_PRESSURE, data);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- break;
- default:
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- break;
- }
-
- return result;
-}
-
-/**
- * Request slave configuration information
- *
- * Use this specifically after requesting a slave configuration to see what the
- * slave accually accepted.
- *
- * @param mldl_cfg pointer to the mldl configuration structure
- * @param gyro_handle handle to the gyro sensor
- * @param slave_handle handle to the slave sensor
- * @param data the data being requested.
- * @param slave slave description
- * @param pdata slave platform data
- *
- * @return 0 or non-zero error code
- */
-int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
- void *gyro_handle,
- void *slave_handle,
- struct ext_slave_config *data,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata)
-{
- int result;
- if (!mldl_cfg || !data || !slave) {
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- }
- switch (slave->type) {
- case EXT_SLAVE_TYPE_ACCELEROMETER:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_ACCEL, data);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- break;
- case EXT_SLAVE_TYPE_COMPASS:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_COMPASS, data);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- break;
- case EXT_SLAVE_TYPE_PRESSURE:
- result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_PRESSURE, data);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- break;
- default:
- LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
- return INV_ERROR_INVALID_PARAMETER;
- break;
- }
- return result;
-}
-/**
- *@}
- */
diff --git a/60xx/mlsdk/mllite/mldmp.c b/60xx/mlsdk/mllite/mldmp.c
deleted file mode 100644
index 7381dd4..0000000
--- a/60xx/mlsdk/mllite/mldmp.c
+++ /dev/null
@@ -1,284 +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: mldmp.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @addtogroup MLDMP
- *
- * @{
- * @file mldmp.c
- * @brief Shared functions between all the different DMP versions
-**/
-
-#include <stdio.h>
-
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "mltypes.h"
-#include "ml.h"
-#include "mldl_cfg.h"
-#include "mldl.h"
-#include "compass.h"
-#include "mlSetGyroBias.h"
-#include "mlsl.h"
-#include "mlFIFO.h"
-#include "mldmp.h"
-#include "mlstates.h"
-#include "dmpDefault.h"
-#include "mlFIFOHW.h"
-#include "mlsupervisor.h"
-
-#include "log.h"
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-dmp"
-
-/**
- * @brief Open the default motion sensor engine.
- * This function is used to open the default MPL engine,
- * featuring, for example, sensor fusion (6 axes and 9 axes),
- * sensor calibration, accelerometer data byte swapping, among
- * others.
- * Compare with the other provided engines.
- *
- * @pre inv_serial_start() must have been called to instantiate the serial
- * communication.
- *
- * Example:
- * @code
- * result = inv_dmp_open( );
- * if (INV_SUCCESS != result) {
- * // Handle the error case
- * }
- * @endcode
- *
- * @return Zero on success; Error Code on any failure.
- *
- */
-inv_error_t inv_dmp_open(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- unsigned char state = inv_get_state();
- struct mldl_cfg *mldl_cfg;
- unsigned long requested_sensors;
-
- /*************************************************************
- * Common operations before calling DMPOpen
- ************************************************************/
- if (state == INV_STATE_DMP_OPENED)
- return INV_SUCCESS;
-
- if (state == INV_STATE_DMP_STARTED) {
- return inv_dmp_stop();
- }
-
- result = inv_state_transition(INV_STATE_DMP_OPENED);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- result = inv_dl_open(inv_get_serial_handle());
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#ifdef ML_USE_DMP_SIM
- do {
- void setup_univ();
- setup_univ(); /* hijack the read and write paths
- and re-direct them to the simulator */
- } while (0);
-#endif
-
- result = inv_setup_dmp();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- // Init vars.
- inv_init_ml();
-
- result = inv_init_fifo_param();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_enable_set_bias();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- inv_init_fifo_hardare();
- mldl_cfg = inv_get_dl_config();
- requested_sensors = INV_THREE_AXIS_GYRO;
- if (mldl_cfg->accel && mldl_cfg->accel->resume)
- requested_sensors |= INV_THREE_AXIS_ACCEL;
-
- if (mldl_cfg->compass && mldl_cfg->compass->resume)
- requested_sensors |= INV_THREE_AXIS_COMPASS;
-
- if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
- requested_sensors |= INV_THREE_AXIS_PRESSURE;
-
- result = inv_init_requested_sensors(requested_sensors);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_apply_calibration();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- if (NULL != mldl_cfg->accel){
- result = inv_apply_endian_accel();
- }
-
- return result;
-}
-
-/**
- * @brief Start the DMP.
- *
- * @pre inv_dmp_open() must have been called.
- *
- * @code
- * result = inv_dmp_start();
- * if (INV_SUCCESS != result) {
- * // Handle the error case
- * }
- * @endcode
- *
- * @return INV_SUCCESS if successful, or Non-zero error code otherwise.
- */
-inv_error_t inv_dmp_start(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
-
- if (inv_get_state() == INV_STATE_DMP_STARTED)
- return INV_SUCCESS;
-
- result = inv_state_transition(INV_STATE_DMP_STARTED);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- inv_init_sensor_fusion_supervisor();
- result = inv_dl_start(inv_get_dl_config()->requested_sensors);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- /* This is done after the start since it will modify DMP memory, which
- * will cause a full reset is most cases */
- result = inv_reset_motion();
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-/**
- * @brief Stops the DMP and puts it in low power.
- *
- * @pre inv_dmp_start() must have been called.
- *
- * @return INV_SUCCESS, Non-zero error code otherwise.
- */
-inv_error_t inv_dmp_stop(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
-
- if (inv_get_state() == INV_STATE_DMP_OPENED)
- return INV_SUCCESS;
-
- result = inv_state_transition(INV_STATE_DMP_OPENED);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_dl_stop(INV_ALL_SENSORS);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- return result;
-}
-
-/**
- * @brief Closes the motion sensor engine.
- * Does not close the serial communication. To do that,
- * call inv_serial_stop().
- * After calling inv_dmp_close() another DMP module can be
- * loaded in the MPL with the corresponding necessary
- * intialization and configurations, via any of the
- * MLDmpXXXOpen functions.
- *
- * @pre inv_dmp_open() must have been called.
- *
- * @code
- * result = inv_dmp_close();
- * if (INV_SUCCESS != result) {
- * // Handle the error case
- * }
- * @endcode
- *
- * @return INV_SUCCESS, Non-zero error code otherwise.
- */
-inv_error_t inv_dmp_close(void)
-{
- INVENSENSE_FUNC_START;
- inv_error_t result;
- inv_error_t firstError = INV_SUCCESS;
-
- if (inv_get_state() <= INV_STATE_DMP_CLOSED)
- return INV_SUCCESS;
-
- result = inv_disable_set_bias();
- ERROR_CHECK_FIRST(firstError, result);
-
- result = inv_dl_stop(INV_ALL_SENSORS);
- ERROR_CHECK_FIRST(firstError, result);
-
- result = inv_close_fifo();
- ERROR_CHECK_FIRST(firstError, result);
-
- result = inv_dl_close();
- ERROR_CHECK_FIRST(firstError, result);
-
- result = inv_state_transition(INV_STATE_SERIAL_OPENED);
- ERROR_CHECK_FIRST(firstError, result);
-
- return result;
-}
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/mldmp.h b/60xx/mlsdk/mllite/mldmp.h
deleted file mode 100644
index f519798..0000000
--- a/60xx/mlsdk/mllite/mldmp.h
+++ /dev/null
@@ -1,96 +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: mldmp.h 5629 2011-06-11 03:13:08Z mcaramello $
- ******************************************************************************/
-
-/**
- * @defgroup MLDMP
- * @brief
- *
- * These are the top level functions that define how to load the MPL. In order
- * to use most of the features, the DMP must be loaded with some code. The
- * loading procedure takes place when calling inv_dmp_open with a given DMP set
- * function, after having open the serial communication with the device via
- * inv_serial_start().
- * The DMP set function will load the DMP memory and enable a certain
- * set of features.
- *
- * First select a DMP version from one of the released DMP sets.
- * These could be:
- * - DMP default to load and use the default DMP code featuring pedometer,
- * gestures, and orientation. Use inv_dmp_open().
- * - DMP pedometer stand-alone to load and use the standalone pedometer
- * implementation. Use inv_open_low_power_pedometer().
- * <!-- - DMP EIS ... Use inv_eis_open_dmp(). -->
- *
- * After inv_dmp_openXXX any number of appropriate initialization and configuration
- * routines can be called. Each one of these routines will return an error code
- * and will check to make sure that it is compatible with the the DMP version
- * selected during the call to inv_dmp_open.
- *
- * Once the configuration is complete, make a call to inv_dmp_start(). This will
- * finally turn on the DMP and run the code previously loaded.
- *
- * While the DMP is running, all data fetching, polling or other functions can
- * be called and will return valid data. Some parameteres can be changed while
- * the DMP is runing, while others cannot. Therefore it is important to always
- * check the return code of each function. Check the error code list in mltypes
- * to know what each returned error corresponds to.
- *
- * When no more motion processing is required, the library can be shut down and
- * the DMP turned off. We can do that by calling inv_dmp_close(). Note that
- * inv_dmp_close() will not close the serial communication automatically, which will
- * remain open an active, in case another module needs to be loaded instead.
- * If the intention is shutting down the MPL as well, an explicit call to
- * inv_serial_stop() following inv_dmp_close() has to be made.
- *
- * The MPL additionally implements a basic state machine, whose purpose is to
- * give feedback to the user on whether he is following all the required
- * initialization steps. If an anomalous transition is detected, the user will
- * be warned by a terminal message with the format:
- *
- * <tt>"Error : illegal state transition from STATE_1 to STATE_3"</tt>
- *
- * @{
- * @file mldmp.h
- * @brief Top level entry functions to the MPL library with DMP support
- */
-
-#ifndef MLDMP_H
-#define MLDMP_H
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mldmp_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- inv_error_t inv_dmp_open(void);
- inv_error_t inv_dmp_start(void);
- inv_error_t inv_dmp_stop(void);
- inv_error_t inv_dmp_close(void);
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* MLDMP_H */
-/**
- * @}
-**/
diff --git a/60xx/mlsdk/mllite/mlinclude.h b/60xx/mlsdk/mllite/mlinclude.h
deleted file mode 100644
index dcbe904..0000000
--- a/60xx/mlsdk/mllite/mlinclude.h
+++ /dev/null
@@ -1,50 +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.
- $
- */
-#ifndef INV_INCLUDE_H__
-#define INV_INCLUDE_H__
-
-#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere
-
-#ifdef COVERAGE
-#include "utestCommon.h"
-#endif
-#ifdef PROFILE
-#include "profile.h"
-#endif
-
-#ifdef WIN32
-#ifdef COVERAGE
-
-extern int functionEnterLog(const char *file, const char *func);
-extern int functionExitLog(const char *file, const char *func);
-
-#undef INVENSENSE_FUNC_START
-#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \
- int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__)
-#endif // COVERAGE
-#endif // WIN32
-
-#ifdef PROFILE
-#undef INVENSENSE_FUNC_START
-#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__)
-#define return if ( profileExit(__FILE__, __FUNCTION__) ) return
-#endif // PROFILE
-
-// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return
-
-#endif //INV_INCLUDE_H__
diff --git a/60xx/mlsdk/mllite/mlstates.c b/60xx/mlsdk/mllite/mlstates.c
deleted file mode 100644
index 8ebb086..0000000
--- a/60xx/mlsdk/mllite/mlstates.c
+++ /dev/null
@@ -1,269 +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: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- * @defgroup MLSTATES
- * @brief Basic state machine definition and support for the Motion Library.
- *
- * @{
- * @file mlstates.c
- * @brief The Motion Library state machine definition.
- */
-
-#define ML_C
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-
-#include <stdio.h>
-#include <string.h>
-
-#include "mlstates.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "ml.h"
-#include "mlos.h"
-
-#include <log.h>
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mlstates"
-
-#define _stateDebug(x) //{x}
-
-#define MAX_STATE_CHANGE_PROCESSES (8)
-
-struct state_callback_obj {
- int_fast8_t numStateChangeCallbacks;
- HANDLE mutex;
- state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES];
-};
-
-static struct state_callback_obj sStateChangeCallbacks = { 0 };
-
-/* --------------- */
-/* - Functions. - */
-/* --------------- */
-
-static inv_error_t inv_init_state_callbacks(void)
-{
- memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
- return inv_create_mutex(&sStateChangeCallbacks.mutex);
-}
-
-static inv_error_t MLStateCloseCallbacks(void)
-{
- inv_error_t result;
- result = inv_destroy_mutex(sStateChangeCallbacks.mutex);
- memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
- return result;
-}
-
-/**
- * @internal
- * @brief return a string containing the label assigned to the given state.
- * @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)
-{
- 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;
- }
-}
-
-/**
- * @internal
- * @brief Perform a transition from the current state to newState.
- * Check for the correctness of the transition.
- * Print out an error message if the transition is illegal .
- * This routine is also called if a certain normally constant parameters
- * are changed such as the FIFO Rate.
- * @param newState state we are transitioning to.
- * @return
-**/
-inv_error_t inv_state_transition(unsigned char newState)
-{
- inv_error_t result = INV_SUCCESS;
-
- if (newState == INV_STATE_SERIAL_CLOSED) {
- // Always allow transition to closed
- } else if (newState == INV_STATE_SERIAL_OPENED) {
- inv_init_state_callbacks(); // Always allow first transition to start over
- } else if (((newState == INV_STATE_DMP_OPENED) &&
- ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) ||
- (inv_params_obj.state == INV_STATE_DMP_STARTED)))
- ||
- ((newState == INV_STATE_DMP_STARTED) &&
- (inv_params_obj.state == INV_STATE_DMP_OPENED))) {
- // Valid transitions but no special action required
- } else {
- // All other combinations are illegal
- MPL_LOGE("Error : illegal state transition from %s to %s\n",
- inv_state_name(inv_params_obj.state),
- inv_state_name(newState));
- result = INV_ERROR_SM_TRANSITION;
- }
-
- if (result == INV_SUCCESS) {
- _stateDebug(MPL_LOGV
- ("ML State transition from %s to %s\n",
- inv_state_name(inv_params_obj.state),
- inv_state_name(newState)));
- result = inv_run_state_callbacks(newState);
- if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) {
- MLStateCloseCallbacks();
- }
- inv_params_obj.state = newState;
- }
- return result;
-}
-
-/**
- * @internal
- * @brief To be moved in mlstates.c
-**/
-unsigned char inv_get_state(void)
-{
- return (inv_params_obj.state);
-}
-
-/**
- * @internal
- * @brief This registers a function to be called each time the state
- * changes. It may also be called when the FIFO Rate is changed.
- * It will be called at the start of a state change before the
- * state change has taken place. See Also inv_unregister_state_callback()
- * The FIFO does not have to be on for this callback.
- * @param func Function to be called when a DMP interrupt occurs.
- * @return INV_SUCCESS or non-zero error code.
- */
-
-inv_error_t inv_register_state_callback(state_change_callback_t callback)
-{
- INVENSENSE_FUNC_START;
- int kk;
- inv_error_t result;
-
- result = inv_lock_mutex(sStateChangeCallbacks.mutex);
- if (INV_SUCCESS != result) {
- return result;
- }
- // Make sure we have not filled up our number of allowable callbacks
- if (sStateChangeCallbacks.numStateChangeCallbacks <
- MAX_STATE_CHANGE_PROCESSES) {
- // Make sure we haven't registered this function already
- for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
- if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
- result = INV_ERROR_INVALID_PARAMETER;
- break;
- }
- }
-
- if (INV_SUCCESS == result) {
- // Add new callback
- sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks.
- numStateChangeCallbacks]
- = callback;
- sStateChangeCallbacks.numStateChangeCallbacks++;
- }
- } else {
- result = INV_ERROR_MEMORY_EXAUSTED;
- }
-
- inv_unlock_mutex(sStateChangeCallbacks.mutex);
- return result;
-}
-
-/**
- * @internal
- * @brief This unregisters a function to be called each time the state
- * changes. See Also inv_register_state_callback()
- * The FIFO does not have to be on for this callback.
- * @return INV_SUCCESS or non-zero error code.
- */
-inv_error_t inv_unregister_state_callback(state_change_callback_t callback)
-{
- INVENSENSE_FUNC_START;
- int kk, jj;
- inv_error_t result;
-
- result = inv_lock_mutex(sStateChangeCallbacks.mutex);
- if (INV_SUCCESS != result) {
- return result;
- }
- // Make sure we haven't registered this function already
- result = INV_ERROR_INVALID_PARAMETER;
- for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
- if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
- for (jj = kk + 1;
- jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) {
- sStateChangeCallbacks.stateChangeCallbacks[jj - 1] =
- sStateChangeCallbacks.stateChangeCallbacks[jj];
- }
- sStateChangeCallbacks.numStateChangeCallbacks--;
- result = INV_SUCCESS;
- break;
- }
- }
-
- inv_unlock_mutex(sStateChangeCallbacks.mutex);
- return result;
-}
-
-inv_error_t inv_run_state_callbacks(unsigned char newState)
-{
- int kk;
- inv_error_t result;
-
- result = inv_lock_mutex(sStateChangeCallbacks.mutex);
- if (INV_SUCCESS != result) {
- MPL_LOGE("MLOsLockMutex returned %d\n", result);
- return result;
- }
-
- for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
- if (sStateChangeCallbacks.stateChangeCallbacks[kk]) {
- result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState);
- if (INV_SUCCESS != result) {
- break; // Can't return, must release mutex
- }
- }
- }
-
- inv_unlock_mutex(sStateChangeCallbacks.mutex);
- return result;
-}
diff --git a/60xx/mlsdk/mllite/mlstates.h b/60xx/mlsdk/mllite/mlstates.h
deleted file mode 100644
index cbaa610..0000000
--- a/60xx/mlsdk/mllite/mlstates.h
+++ /dev/null
@@ -1,58 +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: mlstates.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef INV_STATES_H__
-#define INV_STATES_H__
-
-#include "mltypes.h"
-#ifdef INV_INCLUDE_LEGACY_HEADERS
-#include "mlstates_legacy.h"
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* See inv_state_transition for the transition mask */
-#define INV_STATE_SERIAL_CLOSED (0)
-#define INV_STATE_SERIAL_OPENED (1)
-#define INV_STATE_DMP_OPENED (2)
-#define INV_STATE_DMP_STARTED (3)
-#define INV_STATE_DMP_STOPPED (INV_STATE_DMP_OPENED)
-#define INV_STATE_DMP_CLOSED (INV_STATE_SERIAL_OPENED)
-
-#define INV_STATE_NAME(x) (#x)
-
- 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);
- inv_error_t inv_unregister_state_callback(state_change_callback_t callback);
- inv_error_t inv_run_state_callbacks(unsigned char newState);
-
-#ifdef __cplusplus
-}
-#endif
-#endif // INV_STATES_H__
diff --git a/60xx/mlsdk/mllite/mlsupervisor.c b/60xx/mlsdk/mllite/mlsupervisor.c
deleted file mode 100644
index 139297f..0000000
--- a/60xx/mlsdk/mllite/mlsupervisor.c
+++ /dev/null
@@ -1,597 +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: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $
- *
- *****************************************************************************/
-
-/**
- * @defgroup ML_SUPERVISOR
- * @brief Basic sensor fusion supervisor functionalities.
- *
- * @{
- * @file mlsupervisor.c
- * @brief Basic sensor fusion supervisor functionalities.
- */
-
-#include "ml.h"
-#include "mldl.h"
-#include "mldl_cfg.h"
-#include "mltypes.h"
-#include "mlinclude.h"
-#include "compass.h"
-#include "pressure.h"
-#include "dmpKey.h"
-#include "dmpDefault.h"
-#include "mlstates.h"
-#include "mlFIFO.h"
-#include "mlFIFOHW.h"
-#include "mlMathFunc.h"
-#include "mlsupervisor.h"
-#include "mlmath.h"
-
-#include "mlsl.h"
-#include "mlos.h"
-
-#include <log.h>
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-sup"
-
-static unsigned long lastCompassTime = 0;
-static unsigned long polltime = 0;
-static unsigned long coiltime = 0;
-static int accCount = 0;
-static int compassCalStableCount = 0;
-static int compassCalCount = 0;
-static int coiltimerstart = 0;
-static unsigned long disturbtime = 0;
-static int disturbtimerstart = 0;
-
-static yas_filter_if_s f;
-static yas_filter_handle_t handle;
-
-#define SUPERVISOR_DEBUG 0
-
-struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 };
-
-/**
- * @brief This initializes all variables that should be reset on
- */
-void inv_init_sensor_fusion_supervisor(void)
-{
- lastCompassTime = 0;
- polltime = 0;
- inv_obj.acc_state = SF_STARTUP_SETTLE;
- accCount = 0;
- compassCalStableCount = 0;
- compassCalCount = 0;
-
- yas_filter_init(&f);
- f.init(&handle);
-
-#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
- defined CONFIG_MPU_SENSORS_MPU6050B1
- if (inv_compass_present()) {
- struct mldl_cfg *mldl_cfg = inv_get_dl_config();
- if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) {
- (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT);
- }
- }
-#endif
-
- if (ml_supervisor_cb.supervisor_reset_func != NULL) {
- ml_supervisor_cb.supervisor_reset_func();
- }
-}
-
-static int MLUpdateCompassCalibration3DOF(int command, long *data,
- unsigned long deltaTime)
-{
- INVENSENSE_FUNC_START;
- int retValue = INV_SUCCESS;
- static float m[10][10] = { {0} };
- float mInv[10][10] = { {0} };
- float mTmp[10][10] = { {0} };
- static float xTransY[4] = { 0 };
- float magSqr = 0;
- float inpData[3] = { 0 };
- int i, j;
- int a, b;
- float d;
-
- switch (command) {
- case CAL_ADD_DATA:
- inpData[0] = (float)data[0];
- inpData[1] = (float)data[1];
- inpData[2] = (float)data[2];
- m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]);
- m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]);
- m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]);
- m[0][3] += (-2 * inpData[0]);
- m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]);
- m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]);
- m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]);
- m[1][3] += (-2 * inpData[1]);
- m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]);
- m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]);
- m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]);
- m[2][3] += (-2 * inpData[2]);
- m[3][0] += (-2 * inpData[0]);
- m[3][1] += (-2 * inpData[1]);
- m[3][2] += (-2 * inpData[2]);
- m[3][3] += 1.0f;
- magSqr =
- inpData[0] * inpData[0] + inpData[1] * inpData[1] +
- inpData[2] * inpData[2];
- xTransY[0] += (-2 * inpData[0]) * magSqr;
- xTransY[1] += (-2 * inpData[1]) * magSqr;
- xTransY[2] += (-2 * inpData[2]) * magSqr;
- xTransY[3] += magSqr;
- break;
- case CAL_RUN:
- a = 4;
- b = a;
- for (i = 0; i < b; i++) {
- for (j = 0; j < b; j++) {
- a = b;
- inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j);
- mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a);
- }
- }
- a = b;
- d = inv_matrix_det(&m[0][0], &a);
- if (d == 0) {
- return INV_ERROR;
- }
- for (i = 0; i < 3; i++) {
- float tmp = 0;
- for (j = 0; j < 4; j++) {
- tmp += mInv[j][i] / d * xTransY[j];
- }
- inv_obj.compass_test_bias[i] =
- -(long)(tmp * inv_obj.compass_sens / 16384.0f);
- }
- break;
- case CAL_RESET:
- for (i = 0; i < 4; i++) {
- for (j = 0; j < 4; j++) {
- m[i][j] = 0;
- }
- xTransY[i] = 0;
- }
- default:
- break;
- }
- return retValue;
-}
-
-/**
- * Entry point for Sensor Fusion operations
- * @internal
- * @param magFB magnetormeter FB
- * @param accSF accelerometer SF
- */
-static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF,
- unsigned long deltaTime)
-{
- static long prevCompassBias[3] = { 0 };
- static long magMax[3] = {
- -1073741824L,
- -1073741824L,
- -1073741824L
- };
- static long magMin[3] = {
- 1073741824L,
- 1073741824L,
- 1073741824L
- };
- int gyroMag;
- long accMag;
- inv_error_t result;
- int i;
-
- if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) {
- ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime);
- }
-
- gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT;
- accMag = inv_accel_sum_of_sqr();
-
- // Scaling below assumes certain scaling.
-#if ACC_MAG_SQR_SHIFT != 16
-#error
-#endif
-
- if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) {
- result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) {
- //Most basic compass calibration, used only with lite MPL
- if (inv_obj.resetting_compass == 1) {
- for (i = 0; i < 3; i++) {
- magMax[i] = -1073741824L;
- magMin[i] = 1073741824L;
- prevCompassBias[i] = 0;
- }
- compassCalStableCount = 0;
- compassCalCount = 0;
- inv_obj.resetting_compass = 0;
- }
- if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) {
- if (compassCalStableCount < 1000) {
- for (i = 0; i < 3; i++) {
- if (inv_obj.compass_sensor_data[i] > magMax[i]) {
- magMax[i] = inv_obj.compass_sensor_data[i];
- }
- if (inv_obj.compass_sensor_data[i] < magMin[i]) {
- magMin[i] = inv_obj.compass_sensor_data[i];
- }
- }
- MLUpdateCompassCalibration3DOF(CAL_ADD_DATA,
- inv_obj.compass_sensor_data,
- deltaTime);
- compassCalStableCount += deltaTime;
- for (i = 0; i < 3; i++) {
- if (magMax[i] - magMin[i] <
- (long long)40 * 1073741824 / inv_obj.compass_sens) {
- compassCalStableCount = 0;
- }
- }
- } else {
- if (compassCalStableCount >= 1000) {
- if (MLUpdateCompassCalibration3DOF
- (CAL_RUN, inv_obj.compass_sensor_data,
- deltaTime) == INV_SUCCESS) {
- inv_obj.got_compass_bias = 1;
- inv_obj.compass_accuracy = 3;
- for(i=0; i<3; i++) {
- inv_obj.compass_bias_error[i] = 35;
- }
- if (inv_obj.compass_state == SF_UNCALIBRATED)
- inv_obj.compass_state = SF_STARTUP_SETTLE;
- inv_set_compass_bias(inv_obj.compass_test_bias);
- }
- compassCalCount = 0;
- compassCalStableCount = 0;
- for (i = 0; i < 3; i++) {
- magMax[i] = -1073741824L;
- magMin[i] = 1073741824L;
- prevCompassBias[i] = 0;
- }
- MLUpdateCompassCalibration3DOF(CAL_RESET,
- inv_obj.compass_sensor_data,
- deltaTime);
- }
- }
- }
- compassCalCount += deltaTime;
- if (compassCalCount > 20000) {
- compassCalCount = 0;
- compassCalStableCount = 0;
- for (i = 0; i < 3; i++) {
- magMax[i] = -1073741824L;
- magMin[i] = 1073741824L;
- prevCompassBias[i] = 0;
- }
- MLUpdateCompassCalibration3DOF(CAL_RESET,
- inv_obj.compass_sensor_data,
- deltaTime);
- }
- }
-
- if (inv_obj.acc_state != SF_STARTUP_SETTLE) {
- if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) {
- inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing
- accCount = 0;
- } else {
- if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214)
- && ((inv_obj.acc_state == SF_DISTURBANCE)
- || (inv_obj.acc_state == SF_SLOW_SETTLE))) {
- accCount += deltaTime;
- if (accCount > 0) {
- inv_obj.acc_state = SF_FAST_SETTLE;
- accCount = 0;
- }
- } else {
- if (inv_obj.acc_state == SF_DISTURBANCE) {
- accCount += deltaTime;
- if (accCount > 500) {
- inv_obj.acc_state = SF_SLOW_SETTLE;
- accCount = 0;
- }
- } else if (inv_obj.acc_state == SF_SLOW_SETTLE) {
- accCount += deltaTime;
- if (accCount > 1000) {
- inv_obj.acc_state = SF_NORMAL;
- accCount = 0;
- }
- } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
- accCount += deltaTime;
- if (accCount > 250) {
- inv_obj.acc_state = SF_NORMAL;
- accCount = 0;
- }
- }
- }
- }
- }
- if (inv_obj.acc_state == SF_STARTUP_SETTLE) {
- accCount += deltaTime;
- if (accCount > 50) {
- *accSF = 1073741824; //Startup settling
- inv_obj.acc_state = SF_NORMAL;
- accCount = 0;
- }
- } else if ((inv_obj.acc_state == SF_NORMAL)
- || (inv_obj.acc_state == SF_SLOW_SETTLE)) {
- *accSF = inv_obj.accel_sens * 64; //Normal
- } else if ((inv_obj.acc_state == SF_DISTURBANCE)) {
- *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0)
- } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
- *accSF = inv_obj.accel_sens * 512; //Amplify accel
- }
- if (!inv_get_gyro_present()) {
- *accSF = inv_obj.accel_sens * 128;
- }
- return INV_SUCCESS;
-}
-
-/**
- * @brief Entry point for software sensor fusion operations.
- * Manages hardware interaction, calls sensor fusion supervisor for
- * bias calculation.
- * @return error code. INV_SUCCESS if no error occurred.
- */
-inv_error_t inv_accel_compass_supervisor(void)
-{
- inv_error_t result;
- int adjustSensorFusion = 0;
- long accSF = 1073741824;
- static double magFB = 0;
- long magSensorData[3];
- float fcin[3];
- float fcout[3];
-
-
- if (inv_compass_present()) { /* check for compass data */
- int i, j;
- 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 (SUPERVISOR_DEBUG) {
- MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n");
- MPL_LOGV("delta time = %ld\n", ctime - polltime);
- }
- polltime = ctime;
- result = inv_get_compass_data(magSensorData);
- /* external slave wants the data even if there is an error */
- if (result && !inv_obj.external_slave_callback) {
- if (SUPERVISOR_DEBUG) {
- MPL_LOGW("inv_get_compass_data returned %d\n", result);
- }
- } else {
- unsigned long compassTime = inv_get_tick_count();
- unsigned long deltaTime = 1;
-
- if (result == INV_SUCCESS) {
- if (lastCompassTime != 0) {
- deltaTime = compassTime - lastCompassTime;
- }
- lastCompassTime = compassTime;
- adjustSensorFusion = 1;
- if (inv_obj.got_init_compass_bias == 0) {
- inv_obj.got_init_compass_bias = 1;
- for (i = 0; i < 3; i++) {
- inv_obj.init_compass_bias[i] = magSensorData[i];
- }
- }
- for (i = 0; i < 3; i++) {
- inv_obj.compass_sensor_data[i] = (long)magSensorData[i];
- inv_obj.compass_sensor_data[i] -=
- inv_obj.init_compass_bias[i];
- tmp[i] = ((long long)inv_obj.compass_sensor_data[i])
- * inv_obj.compass_sens / 16384;
- tmp[i] -= inv_obj.compass_bias[i];
- tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L;
- }
- for (i = 0; i < 3; i++) {
- tmp64 = 0;
- for (j = 0; j < 3; j++) {
- tmp64 += (long long) tmp[j] *
- inv_obj.compass_cal[i * 3 + j];
- }
- inv_obj.compass_calibrated_data[i] =
- (long) (tmp64 / inv_obj.compass_sens);
- }
- //Additions:
- if ((inv_obj.compass_state == 1) &&
- (inv_obj.compass_overunder == 0)) {
- if (disturbtimerstart == 0) {
- disturbtimerstart = 1;
- disturbtime = ctime;
- }
- } else {
- disturbtimerstart = 0;
- }
-
- if (inv_obj.compass_overunder) {
- if (coiltimerstart == 0) {
- coiltimerstart = 1;
- coiltime = ctime;
- }
- }
- if (coiltimerstart == 1) {
- if (ctime - coiltime > 3000) {
- inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
- inv_set_compass_offset();
- inv_reset_compass_calibration();
- coiltimerstart = 0;
- }
- }
-
- if (disturbtimerstart == 1) {
- if (ctime - disturbtime > 10000) {
- inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
- inv_set_compass_offset();
- inv_reset_compass_calibration();
- MPL_LOGI("Compass reset! inv_reset_compass_calibration \n");
- disturbtimerstart = 0;
- }
- }
- }
- if (inv_obj.external_slave_callback) {
- result = inv_obj.external_slave_callback(&inv_obj);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-
-#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);
-
- 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);
- }
-#endif
-
- if (SUPERVISOR_DEBUG) {
- MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n",
- (float)inv_obj.compass_calibrated_data[0] /
- 65536.f,
- (float)inv_obj.compass_calibrated_data[1] /
- 65536.f,
- (float)inv_obj.compass_calibrated_data[2] /
- 65536.f);
- }
- magFB = 1.0;
- adjustSensorFusion = 1;
- result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
- }
- } else {
- //No compass, but still modify accel
- unsigned long ctime = inv_get_tick_count();
- if (polltime == 0 || ((ctime - polltime) > 80)) { // at the beginning AND every 1/8 second
- unsigned long deltaTime = 1;
- adjustSensorFusion = 1;
- magFB = 0;
- if (polltime != 0) {
- deltaTime = ctime - polltime;
- }
- MLSensorFusionSupervisor(&magFB, &accSF, deltaTime);
- polltime = ctime;
- }
- }
- if (adjustSensorFusion == 1) {
- unsigned char regs[4];
- static int prevAccSF = 1;
-
- if (accSF != prevAccSF) {
- regs[0] = (unsigned char)((accSF >> 24) & 0xff);
- regs[1] = (unsigned char)((accSF >> 16) & 0xff);
- regs[2] = (unsigned char)((accSF >> 8) & 0xff);
- regs[3] = (unsigned char)(accSF & 0xff);
- result = inv_set_mpu_memory(KEY_D_0_96, 4, regs);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- prevAccSF = accSF;
- }
- }
-
- if (ml_supervisor_cb.accel_compass_fusion_func != NULL)
- ml_supervisor_cb.accel_compass_fusion_func(magFB);
-
- return INV_SUCCESS;
-}
-
-/**
- * @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.
- */
-inv_error_t inv_reset_compass_calibration(void)
-{
- if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) {
- if (ml_supervisor_cb.reset_advanced_compass_func != NULL)
- ml_supervisor_cb.reset_advanced_compass_func();
- }
- MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1);
-
- inv_obj.compass_bias_error[0] = P_INIT;
- inv_obj.compass_bias_error[1] = P_INIT;
- inv_obj.compass_bias_error[2] = P_INIT;
- inv_obj.compass_accuracy = 0;
-
- inv_obj.got_compass_bias = 0;
- inv_obj.got_init_compass_bias = 0;
- inv_obj.compass_state = SF_UNCALIBRATED;
- inv_obj.resetting_compass = 1;
-
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/mllite/mlsupervisor.h b/60xx/mlsdk/mllite/mlsupervisor.h
deleted file mode 100644
index 62b427e..0000000
--- a/60xx/mlsdk/mllite/mlsupervisor.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: mlsupervisor.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *****************************************************************************/
-
-#ifndef __INV_SUPERVISOR_H__
-#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.
-#define GYRO_MAG_SQR_SHIFT 6
-// The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = 2^this_number
-#define ACC_MAG_SQR_SHIFT 16
-
-#define CAL_RUN 0
-#define CAL_RESET 1
-#define CAL_CHANGED_DATA 2
-#define CAL_RESET_TIME 3
-#define CAL_ADD_DATA 4
-#define CAL_COMBINE 5
-
-#define P_INIT 100000
-
-#define SF_NORMAL 0
-#define SF_DISTURBANCE 1
-#define SF_FAST_SETTLE 2
-#define SF_SLOW_SETTLE 3
-#define SF_STARTUP_SETTLE 4
-#define SF_UNCALIBRATED 5
-
-struct inv_supervisor_cb_obj {
- void (*accel_compass_fusion_func) (double magFB);
- inv_error_t(*progressive_no_motion_supervisor_func) (unsigned long
- deltaTime);
- inv_error_t(*sensor_fusion_advanced_func) (double *magFB,
- unsigned long deltaTime);
- void (*reset_advanced_compass_func) (void);
- void (*supervisor_reset_func) (void);
-};
-
-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/60xx/mlsdk/mllite/pressure.c b/60xx/mlsdk/mllite/pressure.c
deleted file mode 100644
index 9c162ec..0000000
--- a/60xx/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/60xx/mlsdk/mllite/pressure.h b/60xx/mlsdk/mllite/pressure.h
deleted file mode 100644
index 77c5d43..0000000
--- a/60xx/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/60xx/mlsdk/mlutils/checksum.c b/60xx/mlsdk/mlutils/checksum.c
deleted file mode 100644
index a97477d..0000000
--- a/60xx/mlsdk/mlutils/checksum.c
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "mltypes.h"
-
-/** bernstein hash, from public domain source */
-
-uint32_t inv_checksum(unsigned char *str, int len)
-{
- uint32_t hash = 5381;
- int i, c;
-
- for (i = 0; i < len; i++) {
- c = *(str + i);
- hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
- }
-
- return hash;
-}
diff --git a/60xx/mlsdk/mlutils/checksum.h b/60xx/mlsdk/mlutils/checksum.h
deleted file mode 100644
index 4d3f046..0000000
--- a/60xx/mlsdk/mlutils/checksum.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef MPLCHECKSUM_H
-#define MPLCHECKSUM_H
-
-#ifdef __cplusplus
-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);
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* MPLCHECKSUM_H */
diff --git a/60xx/mlsdk/mlutils/mputest.c b/60xx/mlsdk/mlutils/mputest.c
deleted file mode 100644
index ac0fa30..0000000
--- a/60xx/mlsdk/mlutils/mputest.c
+++ /dev/null
@@ -1,1396 +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)
-
-#ifdef CONFIG_MPU_SENSORS_MPU3050
-/**
- * @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;
-}
-
-#else /* CONFIG_MPU_SENSORS_MPU3050 */
-
-/**
- * @brief Test the gyroscope sensor.
- * Implements the core logic of the MPU Self Test but does not provide
- * a PASS/FAIL output as in the MPU-3050 implementation.
- * @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.
- *
- * @return 0 on success.
- * A non-zero error code on error.
- */
-int inv_test_gyro_6050(void *mlsl_handle,
- short gyro_biases[3], short *temp_avg)
-{
- 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 = 0;
- float Avg[3];
- int i, j, tmp;
- char tmpStr[200];
-
- /* sample rate = 8ms */
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_SMPLRT_DIV, 0x07);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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_CONFIG, regs[0]);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_INT_ENABLE, 0x00);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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 */
-#ifdef CONFIG_MPU_SENSORS_MPU6050A2
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1));
-#else
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_PWR_MGMT_1, j + 1);
-#endif
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- /* wait for 2 ms after switching clock source */
- usleep(2000);
-
- /* enable/reset FIFO */
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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_EN,
- BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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_EN, 0x00);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- /* Getting number of bytes in FIFO */
- result = inv_serial_read(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_FIFO_COUNTH, 2, dataout);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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 {
- 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_EN, 0x00);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-
- /* Read Temperature */
- result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr,
- MPUREG_TEMP_OUT_H, 2, dataout);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return 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);
- }
-
- 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 INV_SUCCESS;
-}
-
-#endif /* CONFIG_MPU_SENSORS_MPU3050 */
-
-#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);
-#ifndef CONFIG_MPU_SENSORS_MPU6050A2
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#endif
- 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 */
-#ifdef CONFIG_MPU_SENSORS_MPU6050A2
- if ((b & BITS_PWRSEL) != BITS_PWRSEL) {
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_PWR_MGM, BITS_PWRSEL);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-#else
- 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;
- }
- }
-#endif
- inv_sleep(60);
-
-#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \
- defined(CONFIG_MPU_SENSORS_MPU6050B1)
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
-#endif
- } else {
- /* restore the power state the part was found in */
-#ifdef CONFIG_MPU_SENSORS_MPU6050A2
- if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) {
- result = inv_serial_single_write(
- mlsl_handle, mputestCfgPtr->addr,
- MPUREG_PWR_MGM, pwr_mgm);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- }
- }
-#else
- 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;
- }
- }
-#endif
- }
-
- 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 */
-#ifdef CONFIG_MPU_SENSORS_MPU3050
- result = inv_test_gyro_3050(mlsl_handle,
- gyro_biases, &temp_avg, provide_result);
-#else
- MPL_LOGW_IF(provide_result,
- "Self Test for MPU-6050 devices is for bias correction only: "
- "no test PASS/FAIL result will be provided\n");
- result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg);
-#endif
-
- 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);
-#if defined CONFIG_MPU_SENSORS_MPU6050B1
- if (MPL_PROD_KEY(mputestCfgPtr->product_id,
- mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) {
- accelSens[2] /= 2;
- } else {
- unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim;
- accelSens[0] /= trim_corr;
- accelSens[1] /= trim_corr;
- accelSens[2] /= trim_corr;
- }
-#endif
- } else {
- /* would be 0, but 1 to avoid divide-by-0 below */
- accelSens[0] = accelSens[1] = accelSens[2] = 1;
- }
-#ifdef CONFIG_MPU_SENSORS_MPU3050
- result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
- provide_result);
-#else
- result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2],
- FALSE);
-#endif
- 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;
-#ifndef CONFIG_MPU_SENSORS_MPU3050
- struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel;
-#endif
-
-#ifdef CONFIG_MPU_SENSORS_MPU3050
- result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23,
- 6, data);
-#else
- result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg,
- slave->read_len, data);
-#endif
- 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/60xx/mlsdk/mlutils/mputest.h b/60xx/mlsdk/mlutils/mputest.h
deleted file mode 100644
index d3347c5..0000000
--- a/60xx/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 */
-
diff --git a/60xx/mlsdk/mlutils/slave.h b/60xx/mlsdk/mlutils/slave.h
deleted file mode 100644
index 45449f6..0000000
--- a/60xx/mlsdk/mlutils/slave.h
+++ /dev/null
@@ -1,188 +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: slave.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef SLAVE_H
-#define SLAVE_H
-
-/**
- * @addtogroup SLAVEDL
- *
- * @{
- * @file slave.h
- * @brief Top level descriptions for Accelerometer support
- *
- */
-
-#include "mltypes.h"
-#include "mpu.h"
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
-/*--- default accel support - selection ---*/
-#define ACCEL_ST_LIS331 0
-#define ACCEL_KIONIX_KXTF9 1
-#define ACCEL_BOSCH 0
-#define ACCEL_ADI 0
-
-#define ACCEL_SLAVEADDR_INVALID 0x00
-
-#define ACCEL_SLAVEADDR_LIS331 0x18
-#define ACCEL_SLAVEADDR_LSM303 0x18
-#define ACCEL_SLAVEADDR_LIS3DH 0x18
-#define ACCEL_SLAVEADDR_KXSD9 0x18
-#define ACCEL_SLAVEADDR_KXTF9 0x0F
-#define ACCEL_SLAVEADDR_BMA150 0x38
-#define ACCEL_SLAVEADDR_BMA222 0x08
-#define ACCEL_SLAVEADDR_BMA250 0x18
-#define ACCEL_SLAVEADDR_ADXL34X 0x53
-#define ACCEL_SLAVEADDR_ADXL34X_ALT 0x1D /* alternative addr */
-#define ACCEL_SLAVEADDR_MMA8450 0x1C
-#define ACCEL_SLAVEADDR_MMA845X 0x1C
-
-#define ACCEL_SLAVEADDR_INVENSENSE 0x68
-/*
- Define default accelerometer to use if no selection is made
-*/
-#if ACCEL_ST_LIS331
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LIS331
-#define DEFAULT_ACCEL_ID ACCEL_ID_LIS331
-#endif
-
-#if ACCEL_ST_LSM303
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LSM303
-#define DEFAULT_ACCEL_ID ACCEL_ID_LSM303A
-#endif
-
-#if ACCEL_KIONIX_KXSD9
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXSD9
-#define DEFAULT_ACCEL_ID ACCEL_ID_KXSD9
-#endif
-
-#if ACCEL_KIONIX_KXTF9
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXTF9
-#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9
-#endif
-
-#if ACCEL_BOSCH
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA150
-#define DEFAULT_ACCEL_ID ACCEL_ID_BMA150
-#endif
-
-#if ACCEL_BMA222
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA222
-#define DEFAULT_ACCEL_ID ACCEL_ID_BMA222
-#endif
-
-#if ACCEL_BOSCH
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA250
-#define DEFAULT_ACCEL_ID ACCEL_ID_BMA250
-#endif
-
-#if ACCEL_ADI
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_ADXL34X
-#define DEFAULT_ACCEL_ID ACCEL_ID_ADXL34X
-#endif
-
-#if ACCEL_MMA8450
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA8450
-#define DEFAULT_ACCEL_ID ACCEL_ID_MMA8450
-#endif
-
-#if ACCEL_MMA845X
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA845X
-#define DEFAULT_ACCEL_ID ACCEL_ID_MMA845X
-#endif
-
-/*--- if no default accelerometer was selected ---*/
-#ifndef DEFAULT_ACCEL_SLAVEADDR
-#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_INVALID
-#endif
-
-#define USE_COMPASS_AICHI 0
-#define USE_COMPASS_AKM 0
-#define USE_COMPASS_YAS529 0
-#define USE_COMPASS_YAS530 0
-#define USE_COMPASS_HMC5883 0
-#define USE_COMPASS_MMC314X 0
-#define USE_COMPASS_HSCDTD002B 0
-#define USE_COMPASS_HSCDTD004A 0
-
-#define COMPASS_SLAVEADDR_INVALID 0x00
-#define COMPASS_SLAVEADDR_AKM_BASE 0x0C
-#define COMPASS_SLAVEADDR_AKM 0x0E
-#define COMPASS_SLAVEADDR_AMI304 0x0E
-#define COMPASS_SLAVEADDR_AMI305 0x0F /*Slave address for AMI 305/306*/
-#define COMPASS_SLAVEADDR_AMI306 0x0E /*Slave address for AMI 305/306*/
-#define COMPASS_SLAVEADDR_YAS529 0x2E
-#define COMPASS_SLAVEADDR_YAS530 0x2E
-#define COMPASS_SLAVEADDR_HMC5883 0x1E
-#define COMPASS_SLAVEADDR_MMC314X 0x30
-#define COMPASS_SLAVEADDR_HSCDTD00XX 0x0C
-
-/*
- Define default compass to use if no selection is made
-*/
- #if USE_COMPASS_AKM
- #define DEFAULT_COMPASS_TYPE COMPASS_ID_AK8975
- #endif
-
- #if USE_COMPASS_AICHI
- #define DEFAULT_COMPASS_TYPE COMPASS_ID_AMI30X
- #endif
-
- #if USE_COMPASS_YAS529
- #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS529
- #endif
-
- #if USE_COMPASS_YAS530
- #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS530
- #endif
-
- #if USE_COMPASS_HMC5883
- #define DEFAULT_COMPASS_TYPE COMPASS_ID_HMC5883
- #endif
-
-#if USE_COMPASS_MMC314X
-#define DEFAULT_COMPASS_TYPE COMPASS_ID_MMC314X
-#endif
-
-#if USE_COMPASS_HSCDTD002B
-#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD002B
-#endif
-
-#if USE_COMPASS_HSCDTD004A
-#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD004A
-#endif
-
-#ifndef DEFAULT_COMPASS_TYPE
-#define DEFAULT_COMPASS_TYPE ID_INVALID
-#endif
-
-
-#endif // SLAVE_H
-
-/**
- * @}
- */
diff --git a/60xx/mlsdk/platform/include/i2c.h b/60xx/mlsdk/platform/include/i2c.h
deleted file mode 100644
index 39dd255..0000000
--- a/60xx/mlsdk/platform/include/i2c.h
+++ /dev/null
@@ -1,133 +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: i2c.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef _I2C_H
-#define _I2C_H
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* ------------ */
-/* - Defines. - */
-/* ------------ */
-
-/* - Error Codes. - */
-
-#define I2C_SUCCESS 0
-#define I2C_ERROR 1
-
-/* ---------- */
-/* - Enums. - */
-/* ---------- */
-
-/* --------------- */
-/* - Structures. - */
-/* --------------- */
-
-#define I2C_RBUFF_MAX 128
-#define I2C_RBUFF_SIZE 17
-
-#ifdef BB_I2C
-#define I2C_RBUFF_DYNAMIC 0
-#else
-#define I2C_RBUFF_DYNAMIC 1
-#endif
-
-typedef struct {
-
- HANDLE i2cHndl;
- HANDLE hDevice; // handle to the drive to be examined
-
- MLU8 readBuffer[1024];
- MLU8 writeBuffer[1024];
-
- MLU16 rBuffRIndex;
- MLU16 rBuffWIndex;
-#if !I2C_RBUFF_DYNAMIC
- MLU8 rBuff[I2C_RBUFF_MAX][I2C_RBUFF_SIZE];
-#else
- MLU8 *rBuff;
-#endif
- MLU16 rBuffMax;
- MLU16 rBuffNumBytes;
-
- MLU8 runThread;
- MLU8 autoProcess;
-
-} I2C_Vars_t;
-
-/* --------------------- */
-/* - Function p-types. - */
-/* --------------------- */
-
-#if (defined(BB_I2C))
-void set_i2c_open_bind_cb(int (*func)(unsigned int i2c_slave_addr));
-void set_i2c_open_cb(int (*func)(const char *dev, int rw));
-void set_i2c_close_cb(int (*func)(int fd));
-void set_i2c_lltransfer_cb(int (*func)(int fd, int client_addr, const char *write_buf, unsigned int write_len,
- char *read_buf, unsigned int read_len ));
-void set_i2c_write_register_cb(int (*func)(int fd, int client_addr, unsigned char reg, unsigned char value));
-void set_i2c_read_register_cb(unsigned char (*func)(int fd, int client_addr, unsigned char reg));
-void set_i2c_dump_register_cb(int (*func)(int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len));
-
-int _i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value);
-unsigned char _i2c_read_register (int fd, int client_addr, unsigned char reg);
-int i2c_lltransfer (int fd, int client_addr, const char *write_buf, unsigned int write_len,
- char *read_buf, unsigned int read_len );
-int i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value);
-unsigned char i2c_read_register (int fd, int client_addr, unsigned char reg);
-int i2c_dump_register (int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len);
-int i2c_open (const char *dev, int rw);
-int i2c_close (int fd);
-int i2c_open_bind (unsigned int i2c_slave_addr);
-#endif
-
-int I2COpen (unsigned char autoProcess, unsigned char createThread);
-int I2CClose (void);
-int I2CDeviceIoControl(void);
-int I2CRead (void);
-int I2CWrite (void);
-int I2CSetBufferSize (unsigned short bufferSize);
-int I2CBufferUpdate (void);
-int I2CHandler (void);
-int I2CReadBuffer (unsigned short cnt, unsigned char bufferMode, unsigned char *rBuff);
-int I2CEmptyBuffer (void);
-int I2CPktsInBuffer (unsigned short *pktsInBuffer);
-int I2CCreateMutex (void);
-int I2CLockMutex (void);
-int I2CUnlockMutex (void);
-
-int I2CWriteBurst (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data);
-int I2CReadBurst (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data);
-
-int I2COpenBB (void);
-int I2CCloseBB (int i2cHandle);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _TEMPLATE_H */
diff --git a/60xx/mlsdk/platform/include/linux/mpu.h b/60xx/mlsdk/platform/include/linux/mpu.h
deleted file mode 100644
index 04fa7b6..0000000
--- a/60xx/mlsdk/platform/include/linux/mpu.h
+++ /dev/null
@@ -1,335 +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.
- $
- */
-
-#ifndef __MPU_H_
-#define __MPU_H_
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#include <linux/ioctl.h>
-#elif defined LINUX
-#include <sys/ioctl.h>
-#endif
-
-/* Number of axes on each sensor */
-#define GYRO_NUM_AXES (3)
-#define ACCEL_NUM_AXES (3)
-#define COMPASS_NUM_AXES (3)
-
-struct mpu_read_write {
- /* Memory address or register address depending on ioctl */
- unsigned short address;
- unsigned short length;
- unsigned char *data;
-};
-
-enum mpuirq_data_type {
- MPUIRQ_DATA_TYPE_MPU_IRQ,
- MPUIRQ_DATA_TYPE_SLAVE_IRQ,
- MPUIRQ_DATA_TYPE_PM_EVENT,
- MPUIRQ_DATA_TYPE_NUM_TYPES,
-};
-
-/* User space PM event notification */
-#define MPU_PM_EVENT_SUSPEND_PREPARE (3)
-#define MPU_PM_EVENT_POST_SUSPEND (4)
-
-struct mpuirq_data {
- int interruptcount;
- unsigned long long irqtime;
- int data_type;
- long data;
-};
-
-enum ext_slave_config_key {
- MPU_SLAVE_CONFIG_ODR_SUSPEND,
- MPU_SLAVE_CONFIG_ODR_RESUME,
- MPU_SLAVE_CONFIG_FSR_SUSPEND,
- MPU_SLAVE_CONFIG_FSR_RESUME,
- MPU_SLAVE_CONFIG_MOT_THS,
- MPU_SLAVE_CONFIG_NMOT_THS,
- MPU_SLAVE_CONFIG_MOT_DUR,
- MPU_SLAVE_CONFIG_NMOT_DUR,
- MPU_SLAVE_CONFIG_IRQ_SUSPEND,
- MPU_SLAVE_CONFIG_IRQ_RESUME,
- MPU_SLAVE_WRITE_REGISTERS,
- MPU_SLAVE_READ_REGISTERS,
- /* AMI 306 specific config keys */
- MPU_SLAVE_PARAM,
- MPU_SLAVE_WINDOW,
- MPU_SLAVE_READWINPARAMS,
- MPU_SLAVE_SEARCHOFFSET,
- /* AKM specific config keys */
- MPU_SLAVE_READ_SCALE,
- /* YAS specific config keys */
- MPU_SLAVE_OFFSET_VALS,
- MPU_SLAVE_RANGE_CHECK,
-
- MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS,
-};
-
-/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */
-enum ext_slave_config_irq_type {
- MPU_SLAVE_IRQ_TYPE_NONE,
- MPU_SLAVE_IRQ_TYPE_MOTION,
- MPU_SLAVE_IRQ_TYPE_DATA_READY,
-};
-
-/* Structure for the following IOCTS's
- * MPU_CONFIG_ACCEL
- * MPU_CONFIG_COMPASS
- * MPU_CONFIG_PRESSURE
- * MPU_GET_CONFIG_ACCEL
- * MPU_GET_CONFIG_COMPASS
- * MPU_GET_CONFIG_PRESSURE
- *
- * @key one of enum ext_slave_config_key
- * @len length of data pointed to by data
- * @apply zero if communication with the chip is not necessary, false otherwise
- * This flag can be used to select cached data or to refresh cashed data
- * cache data to be pushed later or push immediately. If true and the
- * slave is on the secondary bus the MPU will first enger bypass mode
- * before calling the slaves .config or .get_config funcion
- * @data pointer to the data to confgure or get
- */
-struct ext_slave_config {
- int key;
- int len;
- int apply;
- void *data;
-};
-
-enum ext_slave_type {
- EXT_SLAVE_TYPE_GYROSCOPE,
- EXT_SLAVE_TYPE_ACCELEROMETER,
- EXT_SLAVE_TYPE_COMPASS,
- EXT_SLAVE_TYPE_PRESSURE,
- /*EXT_SLAVE_TYPE_TEMPERATURE */
-
- EXT_SLAVE_NUM_TYPES
-};
-
-enum ext_slave_id {
- ID_INVALID = 0,
-
- ACCEL_ID_LIS331,
- ACCEL_ID_LSM303A,
- ACCEL_ID_LIS3DH,
- ACCEL_ID_KXSD9,
- ACCEL_ID_KXTF9,
- ACCEL_ID_BMA150,
- ACCEL_ID_BMA222,
- ACCEL_ID_BMA250,
- ACCEL_ID_ADXL34X,
- ACCEL_ID_MMA8450,
- ACCEL_ID_MMA845X,
- ACCEL_ID_MPU6050,
-
- COMPASS_ID_AK8975,
- COMPASS_ID_AMI30X,
- COMPASS_ID_AMI306,
- COMPASS_ID_YAS529,
- COMPASS_ID_YAS530,
- COMPASS_ID_HMC5883,
- COMPASS_ID_LSM303M,
- COMPASS_ID_MMC314X,
- COMPASS_ID_HSCDTD002B,
- COMPASS_ID_HSCDTD004A,
-
- PRESSURE_ID_BMA085,
-};
-
-enum ext_slave_endian {
- EXT_SLAVE_BIG_ENDIAN,
- EXT_SLAVE_LITTLE_ENDIAN,
- EXT_SLAVE_FS8_BIG_ENDIAN,
- EXT_SLAVE_FS16_BIG_ENDIAN,
-};
-
-enum ext_slave_bus {
- EXT_SLAVE_BUS_INVALID = -1,
- EXT_SLAVE_BUS_PRIMARY = 0,
- EXT_SLAVE_BUS_SECONDARY = 1
-};
-
-
-/**
- * struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050
- * slave devices
- *
- * @get_slave_descr: Function pointer to retrieve the struct ext_slave_descr
- * for this slave
- * @irq: the irq number attached to the slave if any.
- * @adapt_num: the I2C adapter number.
- * @bus: the bus the slave is attached to: enum ext_slave_bus
- * @address: the I2C slave address of the slave device.
- * @orientation: the mounting matrix of the device relative to MPU.
- * @irq_data: private data for the slave irq handler
- * @private_data: additional data, user customizable. Not touched by the MPU
- * driver.
- *
- * The orientation matricies are 3x3 rotation matricies
- * that are applied to the data to rotate from the mounting orientation to the
- * platform orientation. The values must be one of 0, 1, or -1 and each row and
- * column should have exactly 1 non-zero value.
- */
-struct ext_slave_platform_data {
- struct ext_slave_descr *(*get_slave_descr) (void);
- int irq;
- int adapt_num;
- int bus;
- unsigned char address;
- signed char orientation[9];
- void *irq_data;
- void *private_data;
-};
-
-struct fix_pnt_range {
- long mantissa;
- long fraction;
-};
-
-static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng)
-{
- return (long)(rng.mantissa * 1000 + rng.fraction / 10);
-}
-
-struct ext_slave_read_trigger {
- unsigned char reg;
- unsigned char value;
-};
-
-/**
- * struct ext_slave_descr - Description of the slave device for programming.
- *
- * @suspend: function pointer to put the device in suspended state
- * @resume: function pointer to put the device in running state
- * @read: function that reads the device data
- * @init: function used to preallocate memory used by the driver
- * @exit: function used to free memory allocated for the driver
- * @config: function used to configure the device
- * @get_config:function used to get the device's configuration
- *
- * @name: text name of the device
- * @type: device type. enum ext_slave_type
- * @id: enum ext_slave_id
- * @reg: starting register address to retrieve data.
- * @len: length in bytes of the sensor data. Should be 6.
- * @endian: byte order of the data. enum ext_slave_endian
- * @range: full scale range of the slave ouput: struct fix_pnt_range
- * @trigger: If reading data first requires writing a register this is the
- * data to write.
- *
- * Defines the functions and information about the slave the mpu3050 and
- * mpu6050 needs to use the slave device.
- */
-struct ext_slave_descr {
- int (*init) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata);
- int (*exit) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata);
- int (*suspend) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata);
- int (*resume) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata);
- int (*read) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata,
- unsigned char *data);
- int (*config) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata,
- struct ext_slave_config *config);
- int (*get_config) (void *mlsl_handle,
- struct ext_slave_descr *slave,
- struct ext_slave_platform_data *pdata,
- struct ext_slave_config *config);
-
- char *name;
- unsigned char type;
- unsigned char id;
- unsigned char read_reg;
- unsigned int read_len;
- unsigned char endian;
- struct fix_pnt_range range;
- struct ext_slave_read_trigger *trigger;
-};
-
-/**
- * struct mpu_platform_data - Platform data for the mpu driver
- * @int_config: Bits [7:3] of the int config register.
- * @orientation: Orientation matrix of the gyroscope
- * @level_shifter: 0: VLogic, 1: VDD
- * @accel: Accel platform data
- * @compass: Compass platform data
- * @pressure: Pressure platform data
- *
- * Contains platform specific information on how to configure the MPU3050 to
- * work on this platform. The orientation matricies are 3x3 rotation matricies
- * that are applied to the data to rotate from the mounting orientation to the
- * platform orientation. The values must be one of 0, 1, or -1 and each row and
- * column should have exactly 1 non-zero value.
- */
-struct mpu_platform_data {
- unsigned char int_config;
- signed char orientation[GYRO_NUM_AXES * GYRO_NUM_AXES];
- unsigned char level_shifter;
- struct ext_slave_platform_data accel;
- struct ext_slave_platform_data compass;
- struct ext_slave_platform_data pressure;
-};
-
-#if defined __KERNEL__ || defined LINUX
-#define MPU_IOCTL (0x81) /* Magic number for MPU Iocts */
-/* IOCTL commands for /dev/mpu */
-#define MPU_SET_MPU_CONFIG _IOWR(MPU_IOCTL, 0x00, struct mldl_cfg)
-#define MPU_GET_MPU_CONFIG _IOW(MPU_IOCTL, 0x00, struct mldl_cfg)
-
-#define MPU_SET_PLATFORM_DATA _IOWR(MPU_IOCTL, 0x01, struct mldl_cfg)
-
-#define MPU_READ _IOWR(MPU_IOCTL, 0x10, struct mpu_read_write)
-#define MPU_WRITE _IOW(MPU_IOCTL, 0x10, struct mpu_read_write)
-#define MPU_READ_MEM _IOWR(MPU_IOCTL, 0x11, struct mpu_read_write)
-#define MPU_WRITE_MEM _IOW(MPU_IOCTL, 0x11, struct mpu_read_write)
-#define MPU_READ_FIFO _IOWR(MPU_IOCTL, 0x12, struct mpu_read_write)
-#define MPU_WRITE_FIFO _IOW(MPU_IOCTL, 0x12, struct mpu_read_write)
-
-#define MPU_READ_COMPASS _IOR(MPU_IOCTL, 0x12, unsigned char)
-#define MPU_READ_ACCEL _IOR(MPU_IOCTL, 0x13, unsigned char)
-#define MPU_READ_PRESSURE _IOR(MPU_IOCTL, 0x14, unsigned char)
-
-#define MPU_CONFIG_ACCEL _IOW(MPU_IOCTL, 0x20, struct ext_slave_config)
-#define MPU_CONFIG_COMPASS _IOW(MPU_IOCTL, 0x21, struct ext_slave_config)
-#define MPU_CONFIG_PRESSURE _IOW(MPU_IOCTL, 0x22, struct ext_slave_config)
-
-#define MPU_GET_CONFIG_ACCEL _IOWR(MPU_IOCTL, 0x20, struct ext_slave_config)
-#define MPU_GET_CONFIG_COMPASS _IOWR(MPU_IOCTL, 0x21, struct ext_slave_config)
-#define MPU_GET_CONFIG_PRESSURE _IOWR(MPU_IOCTL, 0x22, struct ext_slave_config)
-
-#define MPU_SUSPEND _IO(MPU_IOCTL, 0x30)
-#define MPU_RESUME _IO(MPU_IOCTL, 0x31)
-/* Userspace PM Event response */
-#define MPU_PM_EVENT_HANDLED _IO(MPU_IOCTL, 0x32)
-
-#endif
-
-#endif /* __MPU_H_ */
diff --git a/60xx/mlsdk/platform/include/log.h b/60xx/mlsdk/platform/include/log.h
deleted file mode 100644
index 8485074..0000000
--- a/60xx/mlsdk/platform/include/log.h
+++ /dev/null
@@ -1,344 +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.
- $
- */
-
-/*
- * This file incorporates work covered by the following copyright and
- * permission notice:
- *
- * Copyright (C) 2005 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-/*
- * C/C++ logging functions. See the logging documentation for API details.
- *
- * We'd like these to be available from C code (in case we import some from
- * somewhere), so this has a C interface.
- *
- * The output will be correct when the log file is shared between multiple
- * threads and/or multiple processes so long as the operating system
- * supports O_APPEND. These calls have mutex-protected data structures
- * and so are NOT reentrant. Do not use MPL_LOG in a signal handler.
- */
-#ifndef _LIBS_CUTILS_MPL_LOG_H
-#define _LIBS_CUTILS_MPL_LOG_H
-
-#include "mltypes.h"
-#include <stdarg.h>
-
-#ifdef ANDROID
-#ifdef NDK_BUILD
-#include "log_macros.h"
-#else
-#include <utils/Log.h> /* For the LOG macro */
-#endif
-#endif
-
-#ifdef __KERNEL__
-#include <linux/kernel.h>
-#endif
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
- * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
- * at the top of your source file) to change that behavior.
- */
-#ifndef MPL_LOG_NDEBUG
-#ifdef NDEBUG
-#define MPL_LOG_NDEBUG 1
-#else
-#define MPL_LOG_NDEBUG 0
-#endif
-#endif
-
-#ifdef __KERNEL__
-#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
-#define MPL_LOG_DEFAULT KERN_DEFAULT
-#define MPL_LOG_VERBOSE KERN_CONT
-#define MPL_LOG_DEBUG KERN_NOTICE
-#define MPL_LOG_INFO KERN_INFO
-#define MPL_LOG_WARN KERN_WARNING
-#define MPL_LOG_ERROR KERN_ERR
-#define MPL_LOG_SILENT MPL_LOG_VERBOSE
-
-#else
- /* Based off the log priorities in android
- /system/core/include/android/log.h */
-#define MPL_LOG_UNKNOWN (0)
-#define MPL_LOG_DEFAULT (1)
-#define MPL_LOG_VERBOSE (2)
-#define MPL_LOG_DEBUG (3)
-#define MPL_LOG_INFO (4)
-#define MPL_LOG_WARN (5)
-#define MPL_LOG_ERROR (6)
-#define MPL_LOG_SILENT (8)
-#endif
-
-
-/*
- * This is the local tag used for the following simplified
- * logging macros. You can change this preprocessor definition
- * before using the other macros to change the tag.
- */
-#ifndef MPL_LOG_TAG
-#ifdef __KERNEL__
-#define MPL_LOG_TAG
-#else
-#define MPL_LOG_TAG NULL
-#endif
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGV
-#if MPL_LOG_NDEBUG
-#define MPL_LOGV(fmt, ...) \
- do { \
- if (0) \
- MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
- } while (0)
-#else
-#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef CONDITION
-#define CONDITION(cond) ((cond) != 0)
-#endif
-
-#ifndef MPL_LOGV_IF
-#if MPL_LOG_NDEBUG
-#define MPL_LOGV_IF(cond, fmt, ...) \
- do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
-#else
-#define MPL_LOGV_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-#endif
-
-/*
- * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGD
-#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-
-#ifndef MPL_LOGD_IF
-#define MPL_LOGD_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/*
- * Simplified macro to send an info log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGI
-#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-
-#ifndef MPL_LOGI_IF
-#define MPL_LOGI_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/*
- * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGW
-#ifdef __KERNEL__
-#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef MPL_LOGW_IF
-#define MPL_LOGW_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/*
- * Simplified macro to send an error log message using the current MPL_LOG_TAG.
- */
-#ifndef MPL_LOGE
-#ifdef __KERNEL__
-#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-#ifndef MPL_LOGE_IF
-#define MPL_LOGE_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
- : (void)0)
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Log a fatal error. If the given condition fails, this stops program
- * execution like a normal assertion, but also generating the given message.
- * It is NOT stripped from release builds. Note that the condition test
- * is -inverted- from the normal assert() semantics.
- */
-#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
- ((CONDITION(cond)) \
- ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \
- fmt, ##__VA_ARGS__)) \
- : (void)0)
-
-#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
- (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
-
-/*
- * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
- * are stripped out of release builds.
- */
-#if MPL_LOG_NDEBUG
-#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
- do { \
- if (0) \
- MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
- } while (0)
-#define MPL_LOG_FATAL(fmt, ...) \
- do { \
- if (0) \
- MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \
- } while (0)
-#else
-#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
- MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
-#define MPL_LOG_FATAL(fmt, ...) \
- MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
-#endif
-
-/*
- * Assertion that generates a log message when the assertion fails.
- * Stripped out of release builds. Uses the current MPL_LOG_TAG.
- */
-#define MPL_LOG_ASSERT(cond, fmt, ...) \
- MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
-
-/* --------------------------------------------------------------------- */
-
-/*
- * Basic log message macro.
- *
- * Example:
- * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
- *
- * The second argument may be NULL or "" to indicate the "global" tag.
- */
-#ifndef MPL_LOG
-#define MPL_LOG(priority, tag, fmt, ...) \
- MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
-#endif
-
-/*
- * Log macro that allows you to specify a number for the priority.
- */
-#ifndef MPL_LOG_PRI
-#ifdef ANDROID
-#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- ALOG(priority, tag, fmt, ##__VA_ARGS__)
-#elif defined __KERNEL__
-#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
-#else
-#define MPL_LOG_PRI(priority, tag, fmt, ...) \
- _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
-#endif
-#endif
-
-/*
- * Log macro that allows you to pass in a varargs ("args" is a va_list).
- */
-#ifndef MPL_LOG_PRI_VA
-#ifdef ANDROID
-#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
- android_vprintLog(priority, NULL, tag, fmt, args)
-#elif defined __KERNEL__
-/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
-#else
-#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
- _MLPrintVaLog(priority, NULL, tag, fmt, args)
-#endif
-#endif
-
-/* --------------------------------------------------------------------- */
-
-/*
- * ===========================================================================
- *
- * The stuff in the rest of this file should not be used directly.
- */
-
-#ifndef ANDROID
-int _MLPrintLog(int priority, const char *tag, const char *fmt, ...);
-int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
-/* Final implementation of actual writing to a character device */
-int _MLWriteLog(const char *buf, int buflen);
-#endif
-
-static inline void __print_result_location(int result,
- const char *file,
- const char *func, int line)
-{
- MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
-}
-
-#define LOG_RESULT_LOCATION(condition) \
- do { \
- __print_result_location((int)(condition), __FILE__, \
- __func__, __LINE__); \
- } while (0)
-
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/60xx/mlsdk/platform/include/mlmath.h b/60xx/mlsdk/platform/include/mlmath.h
deleted file mode 100644
index bf54870..0000000
--- a/60xx/mlsdk/platform/include/mlmath.h
+++ /dev/null
@@ -1,107 +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: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-#ifndef _ML_MATH_H_
-#define _ML_MATH_H_
-
-#ifndef MLMATH
-// This define makes Microsoft pickup things like M_PI
-#define _USE_MATH_DEFINES
-#include <math.h>
-
-#ifdef WIN32
-// Microsoft doesn't follow standards
-#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5))))
-#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
-#endif
-
-#else // MLMATH
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-/* MPL needs below functions */
-double ml_asin(double);
-double ml_atan(double);
-double ml_atan2(double, double);
-double ml_log(double);
-double ml_sqrt(double);
-double ml_ceil(double);
-double ml_floor(double);
-double ml_cos(double);
-double ml_sin(double);
-double ml_acos(double);
-#ifdef __cplusplus
-} // extern "C"
-#endif
-
-/*
- * We rename functions here to provide the hook for other
- * customized math functions.
- */
-#define sqrt(x) ml_sqrt(x)
-#define log(x) ml_log(x)
-#define asin(x) ml_asin(x)
-#define atan(x) ml_atan(x)
-#define atan2(x,y) ml_atan2(x,y)
-#define ceil(x) ml_ceil(x)
-#define floor(x) ml_floor(x)
-#define fabs(x) (((x)<0)?-(x):(x))
-#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5))))
-#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f))))
-#define cos(x) ml_cos(x)
-#define sin(x) ml_sin(x)
-#define acos(x) ml_acos(x)
-
-#define pow(x,y) ml_pow(x,y)
-
-#ifdef LINUX
-/* stubs for float version of math functions */
-#define cosf(x) ml_cos(x)
-#define sinf(x) ml_sin(x)
-#define atan2f(x,y) ml_atan2(x,y)
-#define sqrtf(x) ml_sqrt(x)
-#endif
-
-
-
-#endif // MLMATH
-
-#ifndef M_PI
-#define M_PI 3.14159265358979
-#endif
-
-#ifndef ABS
-#define ABS(x) (((x)>=0)?(x):-(x))
-#endif
-
-#ifndef MIN
-#define MIN(x,y) (((x)<(y))?(x):(y))
-#endif
-
-#ifndef MAX
-#define MAX(x,y) (((x)>(y))?(x):(y))
-#endif
-
-/*---------------------------*/
-#endif /* !_ML_MATH_H_ */
diff --git a/60xx/mlsdk/platform/include/mlos.h b/60xx/mlsdk/platform/include/mlos.h
deleted file mode 100644
index 0aeda96..0000000
--- a/60xx/mlsdk/platform/include/mlos.h
+++ /dev/null
@@ -1,109 +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.
- $
- */
-
-#ifndef _MLOS_H
-#define _MLOS_H
-
-#ifndef __KERNEL__
-#include <stdio.h>
-#endif
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#if defined(LINUX) || defined(__KERNEL__)
-#include <stdint.h>
-typedef uintptr_t HANDLE;
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- /* - MLOSCreateFile defines. - */
-
-#define MLOS_GENERIC_READ ((unsigned int)0x80000000)
-#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000)
-#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001)
-#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002)
-#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003)
-
- /* ---------- */
- /* - Enums. - */
- /* ---------- */
-
- /* --------------- */
- /* - Structures. - */
- /* --------------- */
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
-
-#ifndef __KERNEL__
-#include <string.h>
- void *inv_malloc(unsigned int numBytes);
- inv_error_t inv_free(void *ptr);
- inv_error_t inv_create_mutex(HANDLE *mutex);
- inv_error_t inv_lock_mutex(HANDLE mutex);
- inv_error_t inv_unlock_mutex(HANDLE mutex);
- FILE *inv_fopen(char *filename);
- void inv_fclose(FILE *fp);
-
- inv_error_t inv_destroy_mutex(HANDLE handle);
-
- void inv_sleep(int mSecs);
- unsigned long inv_get_tick_count(void);
-
- /* Kernel implmentations */
-#define GFP_KERNEL (0x70)
- static inline void *kmalloc(size_t size,
- unsigned int gfp_flags)
- {
- return inv_malloc((unsigned int)size);
- }
- static inline void *kzalloc(size_t size, unsigned int gfp_flags)
- {
- void *tmp = inv_malloc((unsigned int)size);
- if (tmp)
- memset(tmp, 0, size);
- return tmp;
- }
- static inline void kfree(void *ptr)
- {
- inv_free(ptr);
- }
- static inline void msleep(long msecs)
- {
- inv_sleep(msecs);
- }
- static inline void udelay(unsigned long usecs)
- {
- inv_sleep((usecs + 999) / 1000);
- }
-#else
-#include <linux/delay.h>
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-#endif /* _MLOS_H */
diff --git a/60xx/mlsdk/platform/include/mlsl.h b/60xx/mlsdk/platform/include/mlsl.h
deleted file mode 100644
index 535d117..0000000
--- a/60xx/mlsdk/platform/include/mlsl.h
+++ /dev/null
@@ -1,290 +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.
- $
- */
-
-#ifndef __MLSL_H__
-#define __MLSL_H__
-
-/**
- * @defgroup MLSL
- * @brief Motion Library - Serial Layer.
- * The Motion Library System Layer provides the Motion Library
- * with the communication interface to the hardware.
- *
- * The communication interface is assumed to support serial
- * transfers in burst of variable length up to
- * SERIAL_MAX_TRANSFER_SIZE.
- * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes.
- * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will
- * be subdivided in smaller transfers of length <=
- * SERIAL_MAX_TRANSFER_SIZE.
- * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to
- * overcome any host processor transfer size limitation down to
- * 1 B, the minimum.
- * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor
- * performance and efficiency while requiring higher resource usage
- * (mostly buffering). A smaller value will increase overhead and
- * decrease efficiency but allows to operate with more resource
- * constrained processor and master serial controllers.
- * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
- * mlsl.h header file and master serial controllers.
- * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
- * mlsl.h header file.
- *
- * @{
- * @file mlsl.h
- * @brief The Motion Library System Layer.
- *
- */
-
-#include "mltypes.h"
-#include <linux/mpu.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/*
- * NOTE : to properly support Yamaha compass reads,
- * the max transfer size should be at least 9 B.
- * Length in bytes, typically a power of 2 >= 2
- */
-#define SERIAL_MAX_TRANSFER_SIZE 128
-
-#ifndef __KERNEL__
-/**
- * inv_serial_open() - used to open the serial port.
- * @port The COM port specification associated with the device in use.
- * @sl_handle a pointer to the file handle to the serial device to be open
- * for the communication.
- * This port is used to send and receive data to the device.
- *
- * This function is called by inv_serial_start().
- * Unlike previous MPL Software releases, explicitly calling
- * inv_serial_start() is mandatory to instantiate the communication
- * with the device.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_open(char const *port, void **sl_handle);
-
-/**
- * inv_serial_close() - used to close the serial port.
- * @sl_handle a file handle to the serial device used for the communication.
- *
- * This port is used to send and receive data to the device.
- *
- * This function is called by inv_serial_stop().
- * Unlike previous MPL Software releases, explicitly calling
- * inv_serial_stop() is mandatory to properly shut-down the
- * communication with the device.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_close(void *sl_handle);
-
-/**
- * inv_serial_reset() - used to reset any buffering the driver may be doing
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_reset(void *sl_handle);
-#endif
-
-/**
- * inv_serial_single_write() - used to write a single byte of data.
- * @sl_handle pointer to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @register_addr Register address to write.
- * @data Single byte of data to write.
- *
- * It is called by the MPL to write a single byte of data to the MPU.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_single_write(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned char register_addr,
- unsigned char data);
-
-/**
- * inv_serial_write() - used to write multiple bytes of data to registers.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @register_addr Register address to write.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_write(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short length,
- unsigned char const *data);
-
-/**
- * inv_serial_read() - used to read multiple bytes of data from registers.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @register_addr Register address to read.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_serial_read(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned char register_addr,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_read_mem() - used to read multiple bytes of data from the memory.
- * This should be sent by I2C or SPI.
- *
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @mem_addr The location in the memory to read from.
- * @length Length of burst data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_serial_read_mem(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short mem_addr,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_write_mem() - used to write multiple bytes of data to the memory.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @mem_addr The location in the memory to write to.
- * @length Length of burst data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_serial_write_mem(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short mem_addr,
- unsigned short length,
- unsigned char const *data);
-
-/**
- * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_serial_read_fifo(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short length,
- unsigned char *data);
-
-/**
- * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo.
- * @sl_handle a file handle to the serial device used for the communication.
- * @slave_addr I2C slave address of device.
- * @length Length of burst of data.
- * @data Pointer to block of data.
- *
- * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
- */
-inv_error_t inv_serial_write_fifo(
- void *sl_handle,
- unsigned char slave_addr,
- unsigned short length,
- unsigned char const *data);
-
-#ifndef __KERNEL__
-/**
- * inv_serial_read_cfg() - used to get the configuration data.
- * @cfg Pointer to the configuration data.
- * @len Length of the configuration data.
- *
- * Is called by the MPL to get the configuration data
- * used by the motion library.
- * This data would typically be saved in non-volatile memory.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len);
-
-/**
- * inv_serial_write_cfg() - used to save the configuration data.
- * @cfg Pointer to the configuration data.
- * @len Length of the configuration data.
- *
- * Is called by the MPL to save the configuration data used by the
- * motion library.
- * This data would typically be saved in non-volatile memory.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len);
-
-/**
- * inv_serial_read_cal() - used to get the calibration data.
- * @cfg Pointer to the calibration data.
- * @len Length of the calibration data.
- *
- * It is called by the MPL to get the calibration data used by the
- * motion library.
- * This data is typically be saved in non-volatile memory.
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len);
-
-/**
- * inv_serial_write_cal() - used to save the calibration data.
- *
- * @cfg Pointer to the calibration data.
- * @len Length of the calibration data.
- *
- * It is called by the MPL to save the calibration data used by the
- * motion library.
- * This data is typically be saved in non-volatile memory.
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len);
-
-/**
- * inv_serial_get_cal_length() - Get the calibration length from the storage.
- * @len lenght to be returned
- *
- * returns INV_SUCCESS if successful, a non-zero error code otherwise.
- */
-inv_error_t inv_serial_get_cal_length(unsigned int *len);
-#endif
-#ifdef __cplusplus
-}
-#endif
-/**
- * @}
- */
-#endif /* __MLSL_H__ */
diff --git a/60xx/mlsdk/platform/include/mltypes.h b/60xx/mlsdk/platform/include/mltypes.h
deleted file mode 100644
index 90a126b..0000000
--- a/60xx/mlsdk/platform/include/mltypes.h
+++ /dev/null
@@ -1,265 +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.
- $
- */
-
-/**
- * @defgroup MLERROR
- * @brief Motion Library - Error definitions.
- * Definition of the error codes used within the MPL and
- * returned to the user.
- * Every function tries to return a meaningful error code basing
- * on the occuring error condition. The error code is numeric.
- *
- * The available error codes and their associated values are:
- * - (0) INV_SUCCESS
- * - (1) INV_ERROR
- * - (2) INV_ERROR_INVALID_PARAMETER
- * - (3) INV_ERROR_FEATURE_NOT_ENABLED
- * - (4) INV_ERROR_FEATURE_NOT_IMPLEMENTED
- * - (6) INV_ERROR_DMP_NOT_STARTED
- * - (7) INV_ERROR_DMP_STARTED
- * - (8) INV_ERROR_NOT_OPENED
- * - (9) INV_ERROR_OPENED
- * - (10) INV_ERROR_INVALID_MODULE
- * - (11) INV_ERROR_MEMORY_EXAUSTED
- * - (12) INV_ERROR_DIVIDE_BY_ZERO
- * - (13) INV_ERROR_ASSERTION_FAILURE
- * - (14) INV_ERROR_FILE_OPEN
- * - (15) INV_ERROR_FILE_READ
- * - (16) INV_ERROR_FILE_WRITE
- * - (17) INV_ERROR_INVALID_CONFIGURATION
- * - (20) INV_ERROR_SERIAL_CLOSED
- * - (21) INV_ERROR_SERIAL_OPEN_ERROR
- * - (22) INV_ERROR_SERIAL_READ
- * - (23) INV_ERROR_SERIAL_WRITE
- * - (24) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
- * - (25) INV_ERROR_SM_TRANSITION
- * - (26) INV_ERROR_SM_IMPROPER_STATE
- * - (30) INV_ERROR_FIFO_OVERFLOW
- * - (31) INV_ERROR_FIFO_FOOTER
- * - (32) INV_ERROR_FIFO_READ_COUNT
- * - (33) INV_ERROR_FIFO_READ_DATA
- * - (40) INV_ERROR_MEMORY_SET
- * - (50) INV_ERROR_LOG_MEMORY_ERROR
- * - (51) INV_ERROR_LOG_OUTPUT_ERROR
- * - (60) INV_ERROR_OS_BAD_PTR
- * - (61) INV_ERROR_OS_BAD_HANDLE
- * - (62) INV_ERROR_OS_CREATE_FAILED
- * - (63) INV_ERROR_OS_LOCK_FAILED
- * - (70) INV_ERROR_COMPASS_DATA_OVERFLOW
- * - (71) INV_ERROR_COMPASS_DATA_UNDERFLOW
- * - (72) INV_ERROR_COMPASS_DATA_NOT_READY
- * - (73) INV_ERROR_COMPASS_DATA_ERROR
- * - (75) INV_ERROR_CALIBRATION_LOAD
- * - (76) INV_ERROR_CALIBRATION_STORE
- * - (77) INV_ERROR_CALIBRATION_LEN
- * - (78) INV_ERROR_CALIBRATION_CHECKSUM
- * - (79) INV_ERROR_ACCEL_DATA_OVERFLOW
- * - (80) INV_ERROR_ACCEL_DATA_UNDERFLOW
- * - (81) INV_ERROR_ACCEL_DATA_NOT_READY
- * - (82) INV_ERROR_ACCEL_DATA_ERROR
- *
- * @{
- * @file mltypes.h
- * @}
- */
-
-#ifndef MLTYPES_H
-#define MLTYPES_H
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#else
-#include "stdint_invensense.h"
-#endif
-
-/*---------------------------
- ML Types
----------------------------*/
-
-/**
- * @struct inv_error_t mltypes.h "mltypes"
- * @brief The MPL Error Code return type.
- *
- * @code
- * typedef unsigned char inv_error_t;
- * @endcode
- */
-typedef unsigned char inv_error_t;
-
-#ifndef __cplusplus
-#ifndef __KERNEL__
-typedef int_fast8_t bool;
-#endif
-#endif
-
-/*---------------------------
- ML Defines
----------------------------*/
-
-#ifndef NULL
-#define NULL 0
-#endif
-
-#ifndef TRUE
-#define TRUE 1
-#endif
-
-#ifndef FALSE
-#define FALSE 0
-#endif
-
-#ifndef __KERNEL__
-#ifndef ARRAY_SIZE
-/* Dimension of an array */
-#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0]))
-#endif
-#endif
-/* - ML Errors. - */
-#define ERROR_NAME(x) (#x)
-#define ERROR_CHECK_FIRST(first, x) \
- { if (INV_SUCCESS == first) first = x; }
-
-#define INV_SUCCESS (0)
-/* Generic Error code. Proprietary Error Codes only */
-#define INV_ERROR (1)
-
-/* Compatibility and other generic error codes */
-#define INV_ERROR_INVALID_PARAMETER (2)
-#define INV_ERROR_FEATURE_NOT_ENABLED (3)
-#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (4)
-#define INV_ERROR_DMP_NOT_STARTED (6)
-#define INV_ERROR_DMP_STARTED (7)
-#define INV_ERROR_NOT_OPENED (8)
-#define INV_ERROR_OPENED (9)
-#define INV_ERROR_INVALID_MODULE (10)
-#define INV_ERROR_MEMORY_EXAUSTED (11)
-#define INV_ERROR_DIVIDE_BY_ZERO (12)
-#define INV_ERROR_ASSERTION_FAILURE (13)
-#define INV_ERROR_FILE_OPEN (14)
-#define INV_ERROR_FILE_READ (15)
-#define INV_ERROR_FILE_WRITE (16)
-#define INV_ERROR_INVALID_CONFIGURATION (17)
-
-/* Serial Communication */
-#define INV_ERROR_SERIAL_CLOSED (20)
-#define INV_ERROR_SERIAL_OPEN_ERROR (21)
-#define INV_ERROR_SERIAL_READ (22)
-#define INV_ERROR_SERIAL_WRITE (23)
-#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (24)
-
-/* SM = State Machine */
-#define INV_ERROR_SM_TRANSITION (25)
-#define INV_ERROR_SM_IMPROPER_STATE (26)
-
-/* Fifo */
-#define INV_ERROR_FIFO_OVERFLOW (30)
-#define INV_ERROR_FIFO_FOOTER (31)
-#define INV_ERROR_FIFO_READ_COUNT (32)
-#define INV_ERROR_FIFO_READ_DATA (33)
-
-/* Memory & Registers, Set & Get */
-#define INV_ERROR_MEMORY_SET (40)
-
-#define INV_ERROR_LOG_MEMORY_ERROR (50)
-#define INV_ERROR_LOG_OUTPUT_ERROR (51)
-
-/* OS interface errors */
-#define INV_ERROR_OS_BAD_PTR (60)
-#define INV_ERROR_OS_BAD_HANDLE (61)
-#define INV_ERROR_OS_CREATE_FAILED (62)
-#define INV_ERROR_OS_LOCK_FAILED (63)
-
-/* Compass errors */
-#define INV_ERROR_COMPASS_DATA_OVERFLOW (70)
-#define INV_ERROR_COMPASS_DATA_UNDERFLOW (71)
-#define INV_ERROR_COMPASS_DATA_NOT_READY (72)
-#define INV_ERROR_COMPASS_DATA_ERROR (73)
-
-/* Load/Store calibration */
-#define INV_ERROR_CALIBRATION_LOAD (75)
-#define INV_ERROR_CALIBRATION_STORE (76)
-#define INV_ERROR_CALIBRATION_LEN (77)
-#define INV_ERROR_CALIBRATION_CHECKSUM (78)
-
-/* Accel errors */
-#define INV_ERROR_ACCEL_DATA_OVERFLOW (79)
-#define INV_ERROR_ACCEL_DATA_UNDERFLOW (80)
-#define INV_ERROR_ACCEL_DATA_NOT_READY (81)
-#define INV_ERROR_ACCEL_DATA_ERROR (82)
-
-#ifdef INV_USE_LEGACY_NAMES
-#define ML_SUCCESS INV_SUCCESS
-#define ML_ERROR INV_ERROR
-#define ML_ERROR_INVALID_PARAMETER INV_ERROR_INVALID_PARAMETER
-#define ML_ERROR_FEATURE_NOT_ENABLED INV_ERROR_FEATURE_NOT_ENABLED
-#define ML_ERROR_FEATURE_NOT_IMPLEMENTED INV_ERROR_FEATURE_NOT_IMPLEMENTED
-#define ML_ERROR_DMP_NOT_STARTED INV_ERROR_DMP_NOT_STARTED
-#define ML_ERROR_DMP_STARTED INV_ERROR_DMP_STARTED
-#define ML_ERROR_NOT_OPENED INV_ERROR_NOT_OPENED
-#define ML_ERROR_OPENED INV_ERROR_OPENED
-#define ML_ERROR_INVALID_MODULE INV_ERROR_INVALID_MODULE
-#define ML_ERROR_MEMORY_EXAUSTED INV_ERROR_MEMORY_EXAUSTED
-#define ML_ERROR_DIVIDE_BY_ZERO INV_ERROR_DIVIDE_BY_ZERO
-#define ML_ERROR_ASSERTION_FAILURE INV_ERROR_ASSERTION_FAILURE
-#define ML_ERROR_FILE_OPEN INV_ERROR_FILE_OPEN
-#define ML_ERROR_FILE_READ INV_ERROR_FILE_READ
-#define ML_ERROR_FILE_WRITE INV_ERROR_FILE_WRITE
-#define ML_ERROR_INVALID_CONFIGURATION INV_ERROR_INVALID_CONFIGURATION
-#define ML_ERROR_SERIAL_CLOSED INV_ERROR_SERIAL_CLOSED
-#define ML_ERROR_SERIAL_OPEN_ERROR INV_ERROR_SERIAL_OPEN_ERROR
-#define ML_ERROR_SERIAL_READ INV_ERROR_SERIAL_READ
-#define ML_ERROR_SERIAL_WRITE INV_ERROR_SERIAL_WRITE
-#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED \
- INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
-#define ML_ERROR_SM_TRANSITION INV_ERROR_SM_TRANSITION
-#define ML_ERROR_SM_IMPROPER_STATE INV_ERROR_SM_IMPROPER_STATE
-#define ML_ERROR_FIFO_OVERFLOW INV_ERROR_FIFO_OVERFLOW
-#define ML_ERROR_FIFO_FOOTER INV_ERROR_FIFO_FOOTER
-#define ML_ERROR_FIFO_READ_COUNT INV_ERROR_FIFO_READ_COUNT
-#define ML_ERROR_FIFO_READ_DATA INV_ERROR_FIFO_READ_DATA
-#define ML_ERROR_MEMORY_SET INV_ERROR_MEMORY_SET
-#define ML_ERROR_LOG_MEMORY_ERROR INV_ERROR_LOG_MEMORY_ERROR
-#define ML_ERROR_LOG_OUTPUT_ERROR INV_ERROR_LOG_OUTPUT_ERROR
-#define ML_ERROR_OS_BAD_PTR INV_ERROR_OS_BAD_PTR
-#define ML_ERROR_OS_BAD_HANDLE INV_ERROR_OS_BAD_HANDLE
-#define ML_ERROR_OS_CREATE_FAILED INV_ERROR_OS_CREATE_FAILED
-#define ML_ERROR_OS_LOCK_FAILED INV_ERROR_OS_LOCK_FAILED
-#define ML_ERROR_COMPASS_DATA_OVERFLOW INV_ERROR_COMPASS_DATA_OVERFLOW
-#define ML_ERROR_COMPASS_DATA_UNDERFLOW INV_ERROR_COMPASS_DATA_UNDERFLOW
-#define ML_ERROR_COMPASS_DATA_NOT_READY INV_ERROR_COMPASS_DATA_NOT_READY
-#define ML_ERROR_COMPASS_DATA_ERROR INV_ERROR_COMPASS_DATA_ERROR
-#define ML_ERROR_CALIBRATION_LOAD INV_ERROR_CALIBRATION_LOAD
-#define ML_ERROR_CALIBRATION_STORE INV_ERROR_CALIBRATION_STORE
-#define ML_ERROR_CALIBRATION_LEN INV_ERROR_CALIBRATION_LEN
-#define ML_ERROR_CALIBRATION_CHECKSUM INV_ERROR_CALIBRATION_CHECKSUM
-#define ML_ERROR_ACCEL_DATA_OVERFLOW INV_ERROR_ACCEL_DATA_OVERFLOW
-#define ML_ERROR_ACCEL_DATA_UNDERFLOW INV_ERROR_ACCEL_DATA_UNDERFLOW
-#define ML_ERROR_ACCEL_DATA_NOT_READY INV_ERROR_ACCEL_DATA_NOT_READY
-#define ML_ERROR_ACCEL_DATA_ERROR INV_ERROR_ACCEL_DATA_ERROR
-#endif
-
-/* For Linux coding compliance */
-#ifndef __KERNEL__
-#define EXPORT_SYMBOL(x)
-#endif
-
-/*---------------------------
- p-Types
----------------------------*/
-
-#endif /* MLTYPES_H */
diff --git a/60xx/mlsdk/platform/include/mpu3050.h b/60xx/mlsdk/platform/include/mpu3050.h
deleted file mode 100644
index c363a00..0000000
--- a/60xx/mlsdk/platform/include/mpu3050.h
+++ /dev/null
@@ -1,255 +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.
- $
- */
-
-#ifndef __MPU_H_
-#error Do not include this file directly. Include mpu.h instead.
-#endif
-
-#ifndef __MPU3050_H_
-#define __MPU3050_H_
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#endif
-
-#if !defined CONFIG_MPU_SENSORS_MPU3050
-#error MPU6000 build including MPU3050 header
-#endif
-
-#define MPU_NAME "mpu3050"
-#define DEFAULT_MPU_SLAVEADDR 0x68
-
-/*==== MPU REGISTER SET ====*/
-enum mpu_register {
- MPUREG_WHO_AM_I = 0, /* 00 0x00 */
- MPUREG_PRODUCT_ID, /* 01 0x01 */
- MPUREG_02_RSVD, /* 02 0x02 */
- MPUREG_03_RSVD, /* 03 0x03 */
- MPUREG_04_RSVD, /* 04 0x04 */
- MPUREG_XG_OFFS_TC, /* 05 0x05 */
- MPUREG_06_RSVD, /* 06 0x06 */
- MPUREG_07_RSVD, /* 07 0x07 */
- MPUREG_YG_OFFS_TC, /* 08 0x08 */
- MPUREG_09_RSVD, /* 09 0x09 */
- MPUREG_0A_RSVD, /* 10 0x0a */
- MPUREG_ZG_OFFS_TC, /* 11 0x0b */
- MPUREG_X_OFFS_USRH, /* 12 0x0c */
- MPUREG_X_OFFS_USRL, /* 13 0x0d */
- MPUREG_Y_OFFS_USRH, /* 14 0x0e */
- MPUREG_Y_OFFS_USRL, /* 15 0x0f */
- MPUREG_Z_OFFS_USRH, /* 16 0x10 */
- MPUREG_Z_OFFS_USRL, /* 17 0x11 */
- MPUREG_FIFO_EN1, /* 18 0x12 */
- MPUREG_FIFO_EN2, /* 19 0x13 */
- MPUREG_AUX_SLV_ADDR, /* 20 0x14 */
- MPUREG_SMPLRT_DIV, /* 21 0x15 */
- MPUREG_DLPF_FS_SYNC, /* 22 0x16 */
- MPUREG_INT_CFG, /* 23 0x17 */
- MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */
- MPUREG_19_RSVD, /* 25 0x19 */
- MPUREG_INT_STATUS, /* 26 0x1a */
- MPUREG_TEMP_OUT_H, /* 27 0x1b */
- MPUREG_TEMP_OUT_L, /* 28 0x1c */
- MPUREG_GYRO_XOUT_H, /* 29 0x1d */
- MPUREG_GYRO_XOUT_L, /* 30 0x1e */
- MPUREG_GYRO_YOUT_H, /* 31 0x1f */
- MPUREG_GYRO_YOUT_L, /* 32 0x20 */
- MPUREG_GYRO_ZOUT_H, /* 33 0x21 */
- MPUREG_GYRO_ZOUT_L, /* 34 0x22 */
- MPUREG_23_RSVD, /* 35 0x23 */
- MPUREG_24_RSVD, /* 36 0x24 */
- MPUREG_25_RSVD, /* 37 0x25 */
- MPUREG_26_RSVD, /* 38 0x26 */
- MPUREG_27_RSVD, /* 39 0x27 */
- MPUREG_28_RSVD, /* 40 0x28 */
- MPUREG_29_RSVD, /* 41 0x29 */
- MPUREG_2A_RSVD, /* 42 0x2a */
- MPUREG_2B_RSVD, /* 43 0x2b */
- MPUREG_2C_RSVD, /* 44 0x2c */
- MPUREG_2D_RSVD, /* 45 0x2d */
- MPUREG_2E_RSVD, /* 46 0x2e */
- MPUREG_2F_RSVD, /* 47 0x2f */
- MPUREG_30_RSVD, /* 48 0x30 */
- MPUREG_31_RSVD, /* 49 0x31 */
- MPUREG_32_RSVD, /* 50 0x32 */
- MPUREG_33_RSVD, /* 51 0x33 */
- MPUREG_34_RSVD, /* 52 0x34 */
- MPUREG_DMP_CFG_1, /* 53 0x35 */
- MPUREG_DMP_CFG_2, /* 54 0x36 */
- MPUREG_BANK_SEL, /* 55 0x37 */
- MPUREG_MEM_START_ADDR, /* 56 0x38 */
- MPUREG_MEM_R_W, /* 57 0x39 */
- MPUREG_FIFO_COUNTH, /* 58 0x3a */
- MPUREG_FIFO_COUNTL, /* 59 0x3b */
- MPUREG_FIFO_R_W, /* 60 0x3c */
- MPUREG_USER_CTRL, /* 61 0x3d */
- MPUREG_PWR_MGM, /* 62 0x3e */
- MPUREG_3F_RSVD, /* 63 0x3f */
- NUM_OF_MPU_REGISTERS /* 64 0x40 */
-};
-
-/*==== BITS FOR MPU ====*/
-
-/*---- MPU 'FIFO_EN1' register (12) ----*/
-#define BIT_TEMP_OUT 0x80
-#define BIT_GYRO_XOUT 0x40
-#define BIT_GYRO_YOUT 0x20
-#define BIT_GYRO_ZOUT 0x10
-#define BIT_ACCEL_XOUT 0x08
-#define BIT_ACCEL_YOUT 0x04
-#define BIT_ACCEL_ZOUT 0x02
-#define BIT_AUX_1OUT 0x01
-/*---- MPU 'FIFO_EN2' register (13) ----*/
-#define BIT_AUX_2OUT 0x02
-#define BIT_AUX_3OUT 0x01
-/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/
-#define BITS_EXT_SYNC_NONE 0x00
-#define BITS_EXT_SYNC_TEMP 0x20
-#define BITS_EXT_SYNC_GYROX 0x40
-#define BITS_EXT_SYNC_GYROY 0x60
-#define BITS_EXT_SYNC_GYROZ 0x80
-#define BITS_EXT_SYNC_ACCELX 0xA0
-#define BITS_EXT_SYNC_ACCELY 0xC0
-#define BITS_EXT_SYNC_ACCELZ 0xE0
-#define BITS_EXT_SYNC_MASK 0xE0
-#define BITS_FS_250DPS 0x00
-#define BITS_FS_500DPS 0x08
-#define BITS_FS_1000DPS 0x10
-#define BITS_FS_2000DPS 0x18
-#define BITS_FS_MASK 0x18
-#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
-#define BITS_DLPF_CFG_188HZ 0x01
-#define BITS_DLPF_CFG_98HZ 0x02
-#define BITS_DLPF_CFG_42HZ 0x03
-#define BITS_DLPF_CFG_20HZ 0x04
-#define BITS_DLPF_CFG_10HZ 0x05
-#define BITS_DLPF_CFG_5HZ 0x06
-#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
-#define BITS_DLPF_CFG_MASK 0x07
-/*---- MPU 'INT_CFG' register (17) ----*/
-#define BIT_ACTL 0x80
-#define BIT_ACTL_LOW 0x80
-#define BIT_ACTL_HIGH 0x00
-#define BIT_OPEN 0x40
-#define BIT_OPEN_DRAIN 0x40
-#define BIT_PUSH_PULL 0x00
-#define BIT_LATCH_INT_EN 0x20
-#define BIT_INT_PULSE_WIDTH_50US 0x00
-#define BIT_INT_ANYRD_2CLEAR 0x10
-#define BIT_INT_STAT_READ_2CLEAR 0x00
-#define BIT_MPU_RDY_EN 0x04
-#define BIT_DMP_INT_EN 0x02
-#define BIT_RAW_RDY_EN 0x01
-/*---- MPU 'INT_STATUS' register (1A) ----*/
-#define BIT_INT_STATUS_FIFO_OVERLOW 0x80
-#define BIT_MPU_RDY 0x04
-#define BIT_DMP_INT 0x02
-#define BIT_RAW_RDY 0x01
-/*---- MPU 'BANK_SEL' register (37) ----*/
-#define BIT_PRFTCH_EN 0x20
-#define BIT_CFG_USER_BANK 0x10
-#define BITS_MEM_SEL 0x0f
-/*---- MPU 'USER_CTRL' register (3D) ----*/
-#define BIT_DMP_EN 0x80
-#define BIT_FIFO_EN 0x40
-#define BIT_AUX_IF_EN 0x20
-#define BIT_AUX_RD_LENG 0x10
-#define BIT_AUX_IF_RST 0x08
-#define BIT_DMP_RST 0x04
-#define BIT_FIFO_RST 0x02
-#define BIT_GYRO_RST 0x01
-/*---- MPU 'PWR_MGM' register (3E) ----*/
-#define BIT_H_RESET 0x80
-#define BIT_SLEEP 0x40
-#define BIT_STBY_XG 0x20
-#define BIT_STBY_YG 0x10
-#define BIT_STBY_ZG 0x08
-#define BITS_CLKSEL 0x07
-
-/*---- MPU Silicon Revision ----*/
-#define MPU_SILICON_REV_A4 1 /* MPU A4 Device */
-#define MPU_SILICON_REV_B1 2 /* MPU B1 Device */
-#define MPU_SILICON_REV_B4 3 /* MPU B4 Device */
-#define MPU_SILICON_REV_B6 4 /* MPU B6 Device */
-
-/*---- MPU Memory ----*/
-#define MPU_MEM_BANK_SIZE (256)
-#define FIFO_HW_SIZE (512)
-
-enum MPU_MEMORY_BANKS {
- MPU_MEM_RAM_BANK_0 = 0,
- MPU_MEM_RAM_BANK_1,
- MPU_MEM_RAM_BANK_2,
- MPU_MEM_RAM_BANK_3,
- MPU_MEM_NUM_RAM_BANKS,
- MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS,
- /* This one is always last */
- MPU_MEM_NUM_BANKS
-};
-
-/*---- structure containing control variables used by MLDL ----*/
-/*---- MPU clock source settings ----*/
-/*---- MPU filter selections ----*/
-enum mpu_filter {
- MPU_FILTER_256HZ_NOLPF2 = 0,
- MPU_FILTER_188HZ,
- MPU_FILTER_98HZ,
- MPU_FILTER_42HZ,
- MPU_FILTER_20HZ,
- MPU_FILTER_10HZ,
- MPU_FILTER_5HZ,
- MPU_FILTER_2100HZ_NOLPF,
- NUM_MPU_FILTER
-};
-
-enum mpu_fullscale {
- MPU_FS_250DPS = 0,
- MPU_FS_500DPS,
- MPU_FS_1000DPS,
- MPU_FS_2000DPS,
- NUM_MPU_FS
-};
-
-enum mpu_clock_sel {
- MPU_CLK_SEL_INTERNAL = 0,
- MPU_CLK_SEL_PLLGYROX,
- MPU_CLK_SEL_PLLGYROY,
- MPU_CLK_SEL_PLLGYROZ,
- MPU_CLK_SEL_PLLEXT32K,
- MPU_CLK_SEL_PLLEXT19M,
- MPU_CLK_SEL_RESERVED,
- MPU_CLK_SEL_STOP,
- NUM_CLK_SEL
-};
-
-enum mpu_ext_sync {
- MPU_EXT_SYNC_NONE = 0,
- MPU_EXT_SYNC_TEMP,
- MPU_EXT_SYNC_GYROX,
- MPU_EXT_SYNC_GYROY,
- MPU_EXT_SYNC_GYROZ,
- MPU_EXT_SYNC_ACCELX,
- MPU_EXT_SYNC_ACCELY,
- MPU_EXT_SYNC_ACCELZ,
- NUM_MPU_EXT_SYNC
-};
-
-#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \
- ((ext_sync << 5) | (full_scale << 3) | lpf)
-
-#endif /* __MPU3050_H_ */
diff --git a/60xx/mlsdk/platform/include/stdint_invensense.h b/60xx/mlsdk/platform/include/stdint_invensense.h
deleted file mode 100644
index d238813..0000000
--- a/60xx/mlsdk/platform/include/stdint_invensense.h
+++ /dev/null
@@ -1,51 +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.
- $
- */
-#ifndef STDINT_INVENSENSE_H
-#define STDINT_INVENSENSE_H
-
-#ifndef WIN32
-
-#ifdef __KERNEL__
-#include <linux/types.h>
-#else
-#include <stdint.h>
-#endif
-
-#else
-
-#include <windows.h>
-
-typedef char int8_t;
-typedef short int16_t;
-typedef long int32_t;
-
-typedef unsigned char uint8_t;
-typedef unsigned short uint16_t;
-typedef unsigned long uint32_t;
-
-typedef int int_fast8_t;
-typedef int int_fast16_t;
-typedef long int_fast32_t;
-
-typedef unsigned int uint_fast8_t;
-typedef unsigned int uint_fast16_t;
-typedef unsigned long uint_fast32_t;
-
-#endif
-
-#endif // STDINT_INVENSENSE_H
diff --git a/60xx/mlsdk/platform/linux/kernel/mpuirq.h b/60xx/mlsdk/platform/linux/kernel/mpuirq.h
deleted file mode 100644
index 352170b..0000000
--- a/60xx/mlsdk/platform/linux/kernel/mpuirq.h
+++ /dev/null
@@ -1,41 +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.
- $
- */
-
-#ifndef __MPUIRQ__
-#define __MPUIRQ__
-
-#ifdef __KERNEL__
-#include <linux/time.h>
-#include <linux/ioctl.h>
-#include "mldl_cfg.h"
-#else
-#include <sys/ioctl.h>
-#include <sys/time.h>
-#endif
-
-#define MPUIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x40, unsigned long)
-#define MPUIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x41, unsigned long)
-#define MPUIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x42, struct timeval)
-#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long)
-
-#ifdef __KERNEL__
-void mpuirq_exit(void);
-int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg);
-#endif
-
-#endif
diff --git a/60xx/mlsdk/platform/linux/kernel/slaveirq.h b/60xx/mlsdk/platform/linux/kernel/slaveirq.h
deleted file mode 100644
index beb352b..0000000
--- a/60xx/mlsdk/platform/linux/kernel/slaveirq.h
+++ /dev/null
@@ -1,35 +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.
- $
- */
-
-#ifndef __SLAVEIRQ__
-#define __SLAVEIRQ__
-
-#include <linux/mpu.h>
-#include "mpuirq.h"
-
-#define SLAVEIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x50, unsigned long)
-#define SLAVEIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x51, unsigned long)
-#define SLAVEIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x52, unsigned long)
-
-
-void slaveirq_exit(struct ext_slave_platform_data *pdata);
-int slaveirq_init(struct i2c_adapter *slave_adapter,
- struct ext_slave_platform_data *pdata, char *name);
-
-
-#endif
diff --git a/60xx/mlsdk/platform/linux/kernel/timerirq.h b/60xx/mlsdk/platform/linux/kernel/timerirq.h
deleted file mode 100644
index dc3eea2..0000000
--- a/60xx/mlsdk/platform/linux/kernel/timerirq.h
+++ /dev/null
@@ -1,29 +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.
- $
- */
-
-#ifndef __TIMERIRQ__
-#define __TIMERIRQ__
-
-#include <linux/mpu.h>
-
-#define TIMERIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x60, unsigned long)
-#define TIMERIRQ_GET_INTERRUPT_CNT _IOW(MPU_IOCTL, 0x61, unsigned long)
-#define TIMERIRQ_START _IOW(MPU_IOCTL, 0x62, unsigned long)
-#define TIMERIRQ_STOP _IO(MPU_IOCTL, 0x63)
-
-#endif
diff --git a/60xx/mlsdk/platform/linux/log_linux.c b/60xx/mlsdk/platform/linux/log_linux.c
deleted file mode 100644
index 8e75753..0000000
--- a/60xx/mlsdk/platform/linux/log_linux.c
+++ /dev/null
@@ -1,114 +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: log_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
- ******************************************************************************/
-
-/**
- * @defgroup MPL_LOG
- * @brief Logging facility for the MPL
- *
- * @{
- * @file log.c
- * @brief Core logging facility functions.
- *
- *
-**/
-
-#include <stdio.h>
-#include <string.h>
-#include "log.h"
-#include "mltypes.h"
-
-#define LOG_BUFFER_SIZE (256)
-
-#ifdef WIN32
-#define snprintf _snprintf
-#define vsnprintf _vsnprintf
-#endif
-
-int _MLPrintLog (int priority, const char* tag, const char* fmt, ...)
-{
- va_list ap;
- int result;
-
- va_start(ap, fmt);
- result = _MLPrintVaLog(priority,tag,fmt,ap);
- va_end(ap);
-
- return result;
-}
-
-int _MLPrintVaLog(int priority, const char* tag, const char* fmt, va_list args)
-{
- int result;
- char buf[LOG_BUFFER_SIZE];
- char new_fmt[LOG_BUFFER_SIZE];
- char priority_char;
-
- if (NULL == fmt) {
- fmt = "";
- }
-
- switch (priority) {
- case MPL_LOG_UNKNOWN:
- priority_char = 'U';
- break;
- case MPL_LOG_VERBOSE:
- priority_char = 'V';
- break;
- case MPL_LOG_DEBUG:
- priority_char = 'D';
- break;
- case MPL_LOG_INFO:
- priority_char = 'I';
- break;
- case MPL_LOG_WARN:
- priority_char = 'W';
- break;
- case MPL_LOG_ERROR:
- priority_char = 'E';
- break;
- case MPL_LOG_SILENT:
- priority_char = 'S';
- break;
- case MPL_LOG_DEFAULT:
- default:
- priority_char = 'D';
- break;
- };
-
- result = snprintf(new_fmt, sizeof(new_fmt), "%c/%s:%s",
- priority_char, tag, fmt);
- if (result <= 0) {
- return INV_ERROR_LOG_MEMORY_ERROR;
- }
- result = vsnprintf(buf,sizeof(buf),new_fmt, args);
- if (result <= 0) {
- return INV_ERROR_LOG_OUTPUT_ERROR;
- }
-
- result = _MLWriteLog(buf, strlen(buf));
- return INV_SUCCESS;
-}
-
-/**
- * @}
-**/
-
-
diff --git a/60xx/mlsdk/platform/linux/log_printf_linux.c b/60xx/mlsdk/platform/linux/log_printf_linux.c
deleted file mode 100644
index 744a223..0000000
--- a/60xx/mlsdk/platform/linux/log_printf_linux.c
+++ /dev/null
@@ -1,43 +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: log_printf_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- ******************************************************************************/
-
-/**
- * @addtogroup MPL_LOG
- *
- * @{
- * @file log_printf.c
- * @brief printf replacement for _MLWriteLog.
- */
-
-#include <stdio.h>
-#include "log.h"
-
-int _MLWriteLog (const char * buf, int buflen)
-{
- return fputs(buf, stdout);
-}
-
-/**
- * @}
- */
-
diff --git a/60xx/mlsdk/platform/linux/mlos_linux.c b/60xx/mlsdk/platform/linux/mlos_linux.c
deleted file mode 100644
index fd3b002..0000000
--- a/60xx/mlsdk/platform/linux/mlos_linux.c
+++ /dev/null
@@ -1,206 +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: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $
- *
- *******************************************************************************/
-
-/**
- * @defgroup MLOS
- * @brief OS Interface.
- *
- * @{
- * @file mlos.c
- * @brief OS Interface.
-**/
-
-/* ------------- */
-/* - Includes. - */
-/* ------------- */
-
-#include <sys/time.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdlib.h>
-
-#include "stdint_invensense.h"
-
-#include "mlos.h"
-#include <errno.h>
-
-
-/* -------------- */
-/* - Functions. - */
-/* -------------- */
-
-/**
- * @brief Allocate space
- * @param numBytes number of bytes
- * @return pointer to allocated space
-**/
-void *inv_malloc(unsigned int numBytes)
-{
- // Allocate space.
- void *allocPtr = malloc(numBytes);
- return allocPtr;
-}
-
-
-/**
- * @brief Free allocated space
- * @param ptr pointer to space to deallocate
- * @return error code.
-**/
-inv_error_t inv_free(void *ptr)
-{
- // Deallocate space.
- free(ptr);
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief Mutex create function
- * @param mutex pointer to mutex handle
- * @return error code.
- */
-inv_error_t inv_create_mutex(HANDLE *mutex)
-{
- int res;
- pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t));
- if(pm == NULL)
- return INV_ERROR;
-
- res = pthread_mutex_init(pm, NULL);
- if(res == -1) {
- free(pm);
- return INV_ERROR_OS_CREATE_FAILED;
- }
-
- *mutex = (HANDLE)pm;
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief Mutex lock function
- * @param mutex Mutex handle
- * @return error code.
- */
-inv_error_t inv_lock_mutex(HANDLE mutex)
-{
- int res;
- pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
-
- res = pthread_mutex_lock(pm);
- if(res == -1)
- return INV_ERROR_OS_LOCK_FAILED;
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief Mutex unlock function
- * @param mutex mutex handle
- * @return error code.
-**/
-inv_error_t inv_unlock_mutex(HANDLE mutex)
-{
- int res;
- pthread_mutex_t *pm = (pthread_mutex_t*)mutex;
-
- res = pthread_mutex_unlock(pm);
- if(res == -1)
- return INV_ERROR_OS_LOCK_FAILED;
-
- return INV_SUCCESS;
-}
-
-
-/**
- * @brief open file
- * @param filename name of the file to open.
- * @return error code.
- */
-FILE *inv_fopen(char *filename)
-{
- FILE *fp = fopen(filename,"r");
- return fp;
-}
-
-
-/**
- * @brief close the file.
- * @param fp handle to file to close.
- * @return error code.
- */
-void inv_fclose(FILE *fp)
-{
- fclose(fp);
-}
-
-/**
- * @brief Close Handle
- * @param handle handle to the resource.
- * @return Zero if success, an error code otherwise.
- */
-inv_error_t inv_destroy_mutex(HANDLE handle)
-{
- int error;
- pthread_mutex_t *pm = (pthread_mutex_t*)handle;
- error = pthread_mutex_destroy(pm);
- if (error) {
- return errno;
- }
- free((void*) handle);
-
- return INV_SUCCESS;}
-
-
-/**
- * @brief Sleep function.
- */
-void inv_sleep(int mSecs)
-{
- usleep(mSecs*1000);
-}
-
-
-/**
- * @brief get system's internal tick count.
- * Used for time reference.
- * @return current tick count.
- */
-unsigned long inv_get_tick_count()
-{
- struct timeval tv;
-
- if (gettimeofday(&tv, NULL) !=0)
- return 0;
-
- return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL);
-}
-
- /**********************/
- /** @} */ /* defgroup */
-/**********************/
-
diff --git a/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c
deleted file mode 100644
index 1bd71de..0000000
--- a/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c
+++ /dev/null
@@ -1,497 +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: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
- *****************************************************************************/
-
-/**
- * @defgroup MLSL (Motion Library - Serial Layer)
- * @brief The Motion Library System Layer provides the Motion Library the
- * interface to the system functions.
- *
- * @{
- * @file mlsl_linux_mpu.c
- * @brief The Motion Library System Layer.
- *
- */
-
-/* ------------------ */
-/* - Include Files. - */
-/* ------------------ */
-#include <stdio.h>
-#include <sys/ioctl.h>
-#include <stdlib.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <unistd.h>
-#include <linux/fs.h>
-#include <linux/i2c.h>
-#include <string.h>
-#include <signal.h>
-#include <time.h>
-
-#include "mpu.h"
-#if defined CONFIG_MPU_SENSORS_MPU6050A2
-# include "mpu6050a2.h"
-#elif defined CONFIG_MPU_SENSORS_MPU6050B1
-# include "mpu6050b1.h"
-#elif defined CONFIG_MPU_SENSORS_MPU3050
-# include "mpu3050.h"
-#else
-#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
-#endif
-
-#include "mlsl.h"
-#include "mlos.h"
-#include "mlmath.h"
-#include "mlinclude.h"
-
-#define MLCAL_ID (0x0A0B0C0DL)
-#define MLCAL_FILE "/data/cal.bin"
-#define MLCFG_ID (0x01020304L)
-#define MLCFG_FILE "/data/cfg.bin"
-
-#include <log.h>
-#undef MPL_LOG_TAG
-#define MPL_LOG_TAG "MPL-mlsl"
-
-#ifndef I2CDEV
-#define I2CDEV "/dev/mpu"
-#endif
-
-#define SERIAL_FULL_DEBUG (0)
-
-/* --------------- */
-/* - Prototypes. - */
-/* --------------- */
-
-/* ----------------------- */
-/* - Function Pointers. - */
-/* ----------------------- */
-
-/* --------------------------- */
-/* - Global and Static vars. - */
-/* --------------------------- */
-
-/* ---------------- */
-/* - Definitions. - */
-/* ---------------- */
-
-inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len)
-{
- FILE *fp;
- int bytesRead;
-
- fp = fopen(MLCFG_FILE, "rb");
- if (fp == NULL) {
- MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE);
- return INV_ERROR_FILE_OPEN;
- }
- bytesRead = fread(cfg, 1, len, fp);
- if (bytesRead != len) {
- MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
- bytesRead, len);
- return INV_ERROR_FILE_READ;
- }
- fclose(fp);
-
- if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3])
- != MLCFG_ID) {
- return INV_ERROR_ASSERTION_FAILURE;
- }
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len)
-{
- FILE *fp;
- int bytesWritten;
- unsigned char cfgId[4];
-
- fp = fopen(MLCFG_FILE,"wb");
- if (fp == NULL) {
- MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE);
- return INV_ERROR_FILE_OPEN;
- }
-
- cfgId[0] = (unsigned char)(MLCFG_ID >> 24);
- cfgId[1] = (unsigned char)(MLCFG_ID >> 16);
- cfgId[2] = (unsigned char)(MLCFG_ID >> 8);
- cfgId[3] = (unsigned char)(MLCFG_ID);
- bytesWritten = fwrite(cfgId, 1, 4, fp);
- if (bytesWritten != 4) {
- MPL_LOGE("CFG ID could not be written on file\n");
- return INV_ERROR_FILE_WRITE;
- }
-
- bytesWritten = fwrite(cfg, 1, len, fp);
- if (bytesWritten != len) {
- MPL_LOGE("bytes write (%d) don't match requested length (%d)\n",
- bytesWritten, len);
- return INV_ERROR_FILE_WRITE;
- }
-
- fclose(fp);
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len)
-{
- FILE *fp;
- int bytesRead;
- inv_error_t result = INV_SUCCESS;
-
- fp = fopen(MLCAL_FILE,"rb");
- if (fp == NULL) {
- MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
- return INV_ERROR_FILE_OPEN;
- }
- bytesRead = fread(cal, 1, len, fp);
- if (bytesRead != len) {
- MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
- bytesRead, len);
- result = INV_ERROR_FILE_READ;
- goto read_cal_end;
- }
-
- /* MLCAL_ID not used
- if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3])
- != MLCAL_ID) {
- result = INV_ERROR_ASSERTION_FAILURE;
- goto read_cal_end;
- }
- */
-read_cal_end:
- fclose(fp);
- return result;
-}
-
-inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len)
-{
- FILE *fp;
- int bytesWritten;
- inv_error_t result = INV_SUCCESS;
-
- fp = fopen(MLCAL_FILE,"wb");
- if (fp == NULL) {
- MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
- return INV_ERROR_FILE_OPEN;
- }
- bytesWritten = fwrite(cal, 1, len, fp);
- if (bytesWritten != len) {
- MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
- bytesWritten, len);
- result = INV_ERROR_FILE_WRITE;
- }
- fclose(fp);
- return result;
-}
-
-inv_error_t inv_serial_get_cal_length(unsigned int *len)
-{
- FILE *calFile;
- *len = 0;
-
- calFile = fopen(MLCAL_FILE, "rb");
- if (calFile == NULL) {
- MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
- return INV_ERROR_FILE_OPEN;
- }
-
- *len += (unsigned char)fgetc(calFile) << 24;
- *len += (unsigned char)fgetc(calFile) << 16;
- *len += (unsigned char)fgetc(calFile) << 8;
- *len += (unsigned char)fgetc(calFile);
-
- fclose(calFile);
-
- if (*len <= 0)
- return INV_ERROR_FILE_READ;
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_open(char const *port, void **sl_handle)
-{
- INVENSENSE_FUNC_START;
-
- if (NULL == port) {
- port = I2CDEV;
- }
- *sl_handle = (void*)(uintptr_t) open(port, O_RDWR);
- if((intptr_t)*sl_handle < 0) {
- /* ERROR HANDLING; you can check errno to see what went wrong */
- MPL_LOGE("inv_serial_open\n");
- MPL_LOGE("I2C Error %d: Cannot open Adapter %s\n", errno, port);
- return INV_ERROR_SERIAL_OPEN_ERROR;
- } else {
- MPL_LOGI("inv_serial_open: %s\n", port);
- }
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_close(void *sl_handle)
-{
- INVENSENSE_FUNC_START;
-
- close((int)(uintptr_t)sl_handle);
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_reset(void *sl_handle)
-{
- return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
-}
-
-inv_error_t inv_serial_single_write(void *sl_handle,
- unsigned char slaveAddr,
- unsigned char registerAddr,
- unsigned char data)
-{
- unsigned char buf[2];
- buf[0] = registerAddr;
- buf[1] = data;
- return inv_serial_write(sl_handle, slaveAddr, 2, buf);
-}
-
-inv_error_t inv_serial_write(void *sl_handle,
- unsigned char slaveAddr,
- unsigned short length,
- unsigned char const *data)
-{
- INVENSENSE_FUNC_START;
- struct mpu_read_write msg;
- inv_error_t result;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- msg.address = 0; /* not used */
- msg.length = length;
- msg.data = (unsigned char*)data;
-
- if ((result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE, &msg))) {
- MPL_LOGE("I2C Error: could not write: R:%02x L:%d %d \n",
- data[0], length, result);
- return result;
- } else if (SERIAL_FULL_DEBUG) {
- char data_buff[4096] = "";
- char strchar[3];
- int ii;
- for (ii = 0; ii < length; ii++) {
- snprintf(strchar, sizeof(strchar), "%02x", data[0]);
- strncat(data_buff, strchar, sizeof(data_buff));
- }
- MPL_LOGI("I2C Write Success %02x %02x: %s \n",
- data[0], length, data_buff);
- }
-
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_read(void *sl_handle,
- unsigned char slaveAddr,
- unsigned char registerAddr,
- unsigned short length,
- unsigned char *data)
-{
- INVENSENSE_FUNC_START;
- int result = INV_SUCCESS;
- struct mpu_read_write msg;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- msg.address = registerAddr;
- msg.length = length;
- msg.data = data;
-
- result = ioctl((int)(uintptr_t)sl_handle, MPU_READ, &msg);
-
- if (result != INV_SUCCESS) {
- MPL_LOGE("I2C Error %08x: could not read: R:%02x L:%d\n",
- result, registerAddr, length);
- result = INV_ERROR_SERIAL_READ;
- } else if (SERIAL_FULL_DEBUG) {
- char data_buff[4096] = "";
- char strchar[3];
- int ii;
- for (ii = 0; ii < length; ii++) {
- snprintf(strchar, sizeof(strchar), "%02x", data[0]);
- strncat(data_buff, strchar, sizeof(data_buff));
- }
- MPL_LOGI("I2C Read Success %02x %02x: %s \n",
- registerAddr, length, data_buff);
- }
-
- return (inv_error_t) result;
-}
-
-inv_error_t inv_serial_write_mem(void *sl_handle,
- unsigned char mpu_addr,
- unsigned short memAddr,
- unsigned short length,
- const unsigned char *data)
-{
- INVENSENSE_FUNC_START;
- struct mpu_read_write msg;
- int result;
-
- msg.address = memAddr;
- msg.length = length;
- msg.data = (unsigned char *)data;
-
- result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_MEM, &msg);
- if (result) {
- LOG_RESULT_LOCATION(result);
- return result;
- } else if (SERIAL_FULL_DEBUG) {
- char data_buff[4096] = "";
- char strchar[3];
- int ii;
- for (ii = 0; ii < length; ii++) {
- snprintf(strchar, sizeof(strchar), "%02x", data[0]);
- strncat(data_buff, strchar, sizeof(data_buff));
- }
- MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n",
- memAddr, length, data_buff);
- }
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_read_mem(void *sl_handle,
- unsigned char mpu_addr,
- unsigned short memAddr,
- unsigned short length,
- unsigned char *data)
-{
- INVENSENSE_FUNC_START;
- struct mpu_read_write msg;
- int result;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- msg.address = memAddr;
- msg.length = length;
- msg.data = data;
-
- result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_MEM, &msg);
- if (result != INV_SUCCESS) {
- MPL_LOGE("I2C Error %08x: could not read memory: A:%04x L:%d\n",
- result, memAddr, length);
- return INV_ERROR_SERIAL_READ;
- } else if (SERIAL_FULL_DEBUG) {
- char data_buff[4096] = "";
- char strchar[3];
- int ii;
- for (ii = 0; ii < length; ii++) {
- snprintf(strchar, sizeof(strchar), "%02x", data[0]);
- strncat(data_buff, strchar, sizeof(data_buff));
- }
- MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n",
- memAddr, length, data_buff);
- }
- return INV_SUCCESS;
-}
-
-inv_error_t inv_serial_write_fifo(void *sl_handle,
- unsigned char mpu_addr,
- unsigned short length,
- const unsigned char *data)
-{
- INVENSENSE_FUNC_START;
- struct mpu_read_write msg;
- int result;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- msg.address = 0; /* Not used */
- msg.length = length;
- msg.data = (unsigned char *)data;
-
- result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_FIFO, &msg);
- if (result != INV_SUCCESS) {
- MPL_LOGE("I2C Error: could not write fifo: %02x %02x\n",
- MPUREG_FIFO_R_W, length);
- return INV_ERROR_SERIAL_WRITE;
- } else if (SERIAL_FULL_DEBUG) {
- char data_buff[4096] = "";
- char strchar[3];
- int ii;
- for (ii = 0; ii < length; ii++) {
- snprintf(strchar, sizeof(strchar), "%02x", data[0]);
- strncat(data_buff, strchar, sizeof(data_buff));
- }
- MPL_LOGI("I2C Write Success %02x %02x: %s\n",
- MPUREG_FIFO_R_W, length, data_buff);
- }
- return (inv_error_t) result;
-}
-
-inv_error_t inv_serial_read_fifo(void *sl_handle,
- unsigned char mpu_addr,
- unsigned short length,
- unsigned char *data)
-{
- INVENSENSE_FUNC_START;
- struct mpu_read_write msg;
- int result;
-
- if (NULL == data) {
- return INV_ERROR_INVALID_PARAMETER;
- }
-
- msg.address = MPUREG_FIFO_R_W; /* Not used */
- msg.length = length;
- msg.data = data;
-
- result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_FIFO, &msg);
- if (result != INV_SUCCESS) {
- MPL_LOGE("I2C Error %08x: could not read fifo: R:%02x L:%d\n",
- result, MPUREG_FIFO_R_W, length);
- return INV_ERROR_SERIAL_READ;
- } else if (SERIAL_FULL_DEBUG) {
- char data_buff[4096] = "";
- char strchar[3];
- int ii;
- for (ii = 0; ii < length; ii++) {
- snprintf(strchar, sizeof(strchar), "%02x", data[0]);
- strncat(data_buff, strchar, sizeof(data_buff));
- }
- MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n",
- MPUREG_FIFO_R_W, length, data_buff);
- }
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
-
-
diff --git a/65xx/libsensors_iio/sensors_mpl.cpp b/65xx/libsensors_iio/sensors_mpl.cpp
index e7a3116..5d52b1a 100644
--- a/65xx/libsensors_iio/sensors_mpl.cpp
+++ b/65xx/libsensors_iio/sensors_mpl.cpp
@@ -80,6 +80,7 @@
reserved: {0}
},
get_sensors_list: sensors__get_sensors_list,
+ set_operation_mode: NULL
};
struct sensors_poll_context_t {
diff --git a/Android.mk b/Android.mk
index 65c9e4f..ff26264 100644
--- a/Android.mk
+++ b/Android.mk
@@ -2,14 +2,7 @@
ifneq ($(filter hammerhead, $(TARGET_DEVICE)),)
# hammerhead expects 65xx sensors.
include $(call all-named-subdir-makefiles,65xx)
-else
-ifneq ($(filter guppy dory, $(TARGET_DEVICE)),)
+else ifneq ($(filter guppy dory, $(TARGET_DEVICE)),)
# dory and guppy expect 6515 sensors.
include $(call all-named-subdir-makefiles,6515)
-else
-ifneq ($(filter manta grouper tuna mako, $(TARGET_DEVICE)),)
-# manta, grouper, tuna, and mako expect 60xx sensors.
-include $(call all-named-subdir-makefiles,60xx)
-endif
-endif
endif
