Invensense sensor code, moved to the open-source side
Change-Id: I616c13fdbc2235e400660f6f7a97f879b30879fc
diff --git a/libsensors/Android.mk b/libsensors/Android.mk
new file mode 100644
index 0000000..3745631
--- /dev/null
+++ b/libsensors/Android.mk
@@ -0,0 +1,44 @@
+# 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_CFLAGS := -DLOG_TAG=\"Sensors\"
+LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1
+
+LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp
+
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/include
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/include/linux
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/platform/linux
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/mllite
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/mldmp
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/external/aichi
+LOCAL_C_INCLUDES += hardware/invensense/mlsdk/external/akmd
+
+LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform
+LOCAL_CPPFLAGS+=-DLINUX=1
+LOCAL_LDFLAGS:=-rdynamic
+LOCAL_PRELINK_MODULE := false
+
+include $(BUILD_SHARED_LIBRARY)
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp
new file mode 100644
index 0000000..9ae2254
--- /dev/null
+++ b/libsensors/MPLSensor.cpp
@@ -0,0 +1,1074 @@
+/*
+ * 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 "kernel/timerirq.h"
+#include "kernel/mpuirq.h"
+#include "kernel/slaveirq.h"
+
+extern "C" {
+#include "mlsupervisor.h"
+}
+
+#include "mlcontrol.h"
+
+#define EXTRA_VERBOSE (0)
+#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
+#define VFUNC_LOG LOGV_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))
+
+/* ***************************************************************************
+ * 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), mEnabled(0), mPendingMask(0), mMpuAccuracy(0),
+ mNewData(0), mDmpStarted(false),
+ mMplMutex(PTHREAD_MUTEX_INITIALIZER),
+ mMasterSensorMask(INV_ALL_SENSORS),
+ mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
+ mCurFifoRate(-1), mHaveGoodMpuCal(false),
+ mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
+ mUseTimerirq(false), mSampleCount(0)
+{
+ FUNC_LOG;
+ inv_error_t rv;
+ int mpu_int_fd, i;
+ char *port = NULL;
+
+ LOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors);
+
+ mForceSleep = false;
+
+ for (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) {
+ LOGE("could not open the mpu irq device node");
+ } else {
+ fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK);
+ //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0);
+ 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) {
+ LOGE("could not open the accel irq device node");
+ } else {
+ fcntl(accel_fd, F_SETFL, O_NONBLOCK);
+ //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0);
+ 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) {
+ LOGE("could not open the timer irq device node");
+ } else {
+ fcntl(timer_fd, F_SETFL, O_NONBLOCK);
+ //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0);
+ 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;
+ LOGD("MPLSensor falling back to timerirq for accel 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[RotationVector].acceleration.status
+ = SENSOR_STATUS_ACCURACY_HIGH;
+
+ mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
+ mPendingEvents[LinearAccel].sensor = ID_LA;
+ mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
+ mPendingEvents[LinearAccel].acceleration.status
+ = SENSOR_STATUS_ACCURACY_HIGH;
+
+ mPendingEvents[Gravity].version = sizeof(sensors_event_t);
+ mPendingEvents[Gravity].sensor = ID_GR;
+ mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
+ mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+ mPendingEvents[Gyro].version = sizeof(sensors_event_t);
+ mPendingEvents[Gyro].sensor = ID_GY;
+ mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
+ mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
+ mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
+ mPendingEvents[Accelerometer].sensor = ID_A;
+ mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
+ mPendingEvents[Accelerometer].acceleration.status
+ = SENSOR_STATUS_ACCURACY_HIGH;
+
+ 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) {
+ LOGE("Fatal Error : could not open MPL serial interface");
+ }
+
+ //initialize library parameters
+ initMPL();
+
+ //setup the FIFO contents
+ setupFIFO();
+
+ //we start the motion processing only when a sensor is enabled...
+ //rv = inv_dmp_start();
+ //LOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv);
+ //dmp_started = true;
+
+ pthread_mutex_unlock(&mMplMutex);
+
+}
+
+MPLSensor::~MPLSensor()
+{
+ FUNC_LOG;
+ pthread_mutex_lock(&mMplMutex);
+ if (inv_dmp_stop() != INV_SUCCESS) {
+ LOGD("Error: could not stop the DMP correctly.\n");
+ }
+
+ if (inv_dmp_close() != INV_SUCCESS) {
+ LOGD("Error: could not close the DMP");
+ }
+
+ if (inv_serial_stop() != INV_SUCCESS) {
+ LOGD("Error : could not close the serial port");
+ }
+ pthread_mutex_unlock(&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;
+ int j = 0;
+ if (mPollFds[i].revents & POLLIN) {
+ nread = read(cur_fd, &irqdata, sizeof(irqdata));
+ if (nread > 0) {
+ irq_set[i] = true;
+ //LOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++);
+ }
+ }
+ 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 };
+
+ LOGV(" enabled_sensors: %d dmp_started: %d", enabled_sensors, mDmpStarted);
+
+ 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;
+
+ 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) {
+
+ LOGV_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) {
+ LOGV("inv_set_mpu_sensors: %lx", sen_mask);
+ rv = inv_set_mpu_sensors(sen_mask);
+ LOGE_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)) {
+ LOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
+ mUseTimerirq = true;
+ } else {
+ if (mUseTimerirq) {
+ ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
+ clearIrqData(irq_set);
+ }
+ LOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
+ mUseTimerirq = false;
+ }
+
+ if (!mDmpStarted) {
+ LOGV("Starting DMP");
+ rv = inv_dmp_start();
+ LOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
+ mDmpStarted = true;
+ }
+ }
+
+ //check if we should stop the DMP
+ if (mDmpStarted && (sen_mask == 0)) {
+ LOGV("Stopping DMP");
+ rv = inv_dmp_stop();
+ LOGE_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);
+ if (mHaveGoodMpuCal) {
+ rv = inv_store_calibration();
+ LOGE_IF(rv != INV_SUCCESS,
+ "error: unable to store MPL calibration file");
+ mHaveGoodMpuCal = false;
+ }
+
+ 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;
+ struct mldl_cfg *mldl_cfg;
+
+ if (inv_dmp_open() != INV_SUCCESS) {
+ LOGE("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
+ LOGE_IF(result != INV_SUCCESS,
+ "Fatal Error : could not set enabled sensors.");
+
+ if (inv_load_calibration() != INV_SUCCESS) {
+ LOGE("could not open MPL calibration file");
+ }
+
+ //check for the 9axis fusion library: if available load it and start 9x
+ void* h_dmp_lib=dlopen("libmpl.so", RTLD_LAZY);
+ if(h_dmp_lib) {
+ const char* error;
+ inv_error_t (*fp_inv_enable_9x_fusion)() =
+ (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion");
+ if((error = dlerror()) != NULL) {
+ LOGE("%s", error);
+ } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) {
+ LOGE( "Warning : 9 axis sensor fusion not available "
+ "- No compass detected.\n");
+ }
+ } else {
+ const char* error = dlerror();
+ LOGE("libmpl.so not found, 9x sensor fusion disabled (%s)",error);
+ }
+
+ mldl_cfg = inv_get_dl_config();
+
+ if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
+ LOGE("Error : Bias update function could not be set.\n");
+ }
+
+ if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
+ LOGE("Error : could not set motion interrupt");
+ }
+
+ if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
+ LOGE("Error : could not set fifo interrupt");
+ }
+
+ result = inv_set_fifo_rate(6);
+ if (result != INV_SUCCESS) {
+ LOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
+ }
+
+ mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM;
+ 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) {
+ LOGE("Fatal error: inv_send_accel returned %d\n", result);
+ }
+
+ result = inv_send_quaternion(INV_32_BIT);
+ if (result != INV_SUCCESS) {
+ LOGE("Fatal error: inv_send_quaternion returned %d\n", result);
+ }
+
+ result = inv_send_linear_accel(INV_ALL, INV_32_BIT);
+ if (result != INV_SUCCESS) {
+ LOGE("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) {
+ LOGE("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) {
+ LOGE("Fatal error: inv_send_gravity returned %d\n", result);
+ }
+
+ result = inv_send_gyro(INV_ALL, INV_32_BIT);
+ if (result != INV_SUCCESS) {
+ LOGE("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) {
+ LOGE("Error : Motion callback could not be set.\n");
+
+ }
+
+ if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) {
+ LOGE("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) {
+ mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
+ if (mEnabled & (1 << MPLSensor::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++;
+ //LOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount);
+}
+
+//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;
+ s->gyro.status = mMpuAccuracy;
+ 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;
+ //LOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]);
+ s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
+ if (res == INV_SUCCESS)
+ *pending_mask |= (1 << index);
+}
+
+int MPLSensor::estimateCompassAccuracy()
+{
+ inv_error_t res;
+ int rv;
+
+ res = inv_get_compass_accuracy(&rv);
+ LOGE_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, res2;
+ float bias_error[3];
+ float total_be;
+ static int bias_error_settled = 0;
+
+ res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v);
+
+ if (res != INV_SUCCESS) {
+ LOGD(
+ "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;
+ float ang = 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];
+
+ s->gyro.status
+ = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy
+ : estimateCompassAccuracy());
+}
+
+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;
+ s->gyro.status = mMpuAccuracy;
+ 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;
+ s->gyro.status = mMpuAccuracy;
+ 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 euler[3];
+ float heading[1];
+ float rot_mat[9];
+
+ res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat);
+
+ //ComputeAndOrientation(heading[0], euler, s->orientation.v);
+ calcOrientationSensor(rot_mat, s->orientation.v);
+
+ s->orientation.status
+ = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy
+ : estimateCompassAccuracy());
+
+ if (res == INV_SUCCESS)
+ *pending_mask |= (1 << index);
+ else
+ LOGD("orien_handler: data not valid (%d)", (int) res);
+
+}
+
+int MPLSensor::enable(int32_t handle, int en)
+{
+ FUNC_LOG;
+ LOGV("handle : %d en: %d", handle, en);
+
+ 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;
+ LOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)),
+ "sensor state change what=%d", what);
+
+ pthread_mutex_lock(&mMplMutex);
+ if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
+ uint32_t sensor_type;
+ short flags = newState;
+ mEnabled &= ~(1 << what);
+ mEnabled |= (uint32_t(flags) << what);
+ LOGV_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;
+ /* LOGV_IF(EXTRA_VERBOSE, */
+ LOGE(" 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) {
+ LOGV("set fifo rate: %d %llu", rate, wanted);
+ inv_error_t res; // = inv_dmp_stop();
+ res = inv_set_fifo_rate(rate);
+ LOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
+
+ //res = inv_dmp_start();
+ //LOGE_IF(res != INV_SUCCESS, "error re-starting DMP");
+
+ 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));
+ LOGV_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());
+ LOGV_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);
+ //LOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec);
+ return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+}
+
+int MPLSensor::readEvents(sensors_event_t* data, int count)
+{
+ //VFUNC_LOG;
+ int i;
+ 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) {
+ //LOGV_IF(EXTRA_VERBOSE, "Update Data");
+ rv = inv_update_data();
+ LOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv);
+ }
+
+ else {
+ //probably just one extra read after shutting down
+ LOGV_IF(EXTRA_VERBOSE,
+ "MPLSensor::readEvents called, but there's nothing to do.");
+ }
+
+ pthread_mutex_unlock(&mMplMutex);
+
+ if (!mNewData) {
+ LOGV_IF(EXTRA_VERBOSE, "no new data");
+ return 0;
+ }
+ mNewData = 0;
+ int64_t tt = now_ns();
+ 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 = tt;
+ }
+ }
+
+ 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
+{
+ LOGV("MPLSensor::getFd returning %d", data_fd);
+ return data_fd;
+}
+
+int MPLSensor::getAccelFd() const
+{
+ LOGV("MPLSensor::getAccelFd returning %d", accel_fd);
+ return accel_fd;
+}
+
+int MPLSensor::getTimerFd() const
+{
+ LOGV("MPLSensor::getTimerFd returning %d", timer_fd);
+ return timer_fd;
+}
+
+int MPLSensor::getPowerFd() const
+{
+ int hdl = (int) inv_get_serial_handle();
+ LOGV("MPLSensor::getPowerFd returning %d", hdl);
+ 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 = (int) 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);
+}
diff --git a/libsensors/MPLSensor.h b/libsensors/MPLSensor.h
new file mode 100644
index 0000000..8febac3
--- /dev/null
+++ b/libsensors/MPLSensor.h
@@ -0,0 +1,131 @@
+/*
+ * 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
+ {
+ RotationVector = 0,
+ LinearAccel,
+ Gravity,
+ Gyro,
+ Accelerometer,
+ MagneticField,
+ Orientation,
+ 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();
+ 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 mMpuAccuracy; //global storage for the current accuracy status
+ 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 mUseTimerIrqAccel;
+ bool mUsetimerIrqCompass;
+ bool mUseTimerirq;
+ struct pollfd mPollFds[4];
+ int mSampleCount;
+ pthread_mutex_t mMplMutex;
+ int64_t now_ns();
+
+ 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;
+
+};
+
+void setCallbackObject(MPLSensor*);
+
+/*****************************************************************************/
+
+#endif // ANDROID_MPL_SENSOR_H
diff --git a/libsensors/SensorBase.cpp b/libsensors/SensorBase.cpp
new file mode 100644
index 0000000..d448eb2
--- /dev/null
+++ b/libsensors/SensorBase.cpp
@@ -0,0 +1,128 @@
+/*
+ * 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 <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);
+ 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;
+}
+
+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);
+ LOGE_IF(fd<0, "couldn't find '%s' input device", inputName);
+ return fd;
+}
diff --git a/libsensors/SensorBase.h b/libsensors/SensorBase.h
new file mode 100644
index 0000000..bb4d055
--- /dev/null
+++ b/libsensors/SensorBase.h
@@ -0,0 +1,65 @@
+/*
+ * 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/libsensors/sensors.h b/libsensors/sensors.h
new file mode 100644
index 0000000..283870b
--- /dev/null
+++ b/libsensors/sensors.h
@@ -0,0 +1,53 @@
+/*
+ * 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_RV (ID_MPL_BASE)
+#define ID_LA (ID_RV + 1)
+#define ID_GR (ID_LA + 1)
+#define ID_GY (ID_GR + 1)
+#define ID_A (ID_GY + 1)
+#define ID_M (ID_A + 1)
+#define ID_O (ID_M + 1)
+
+/*****************************************************************************/
+
+/*
+ * The SENSORS Module
+ */
+
+__END_DECLS
+
+#endif // ANDROID_SENSORS_H
diff --git a/libsensors/sensors_mpl.cpp b/libsensors/sensors_mpl.cpp
new file mode 100644
index 0000000..44697bf
--- /dev/null
+++ b/libsensors/sensors_mpl.cpp
@@ -0,0 +1,370 @@
+
+/*
+ * 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 code from this file for Google code check modified by Meenakshi Ramamoorthi on May 31st **********/
+
+#define LOG_NDEBUG 0
+#define LOG_TAG "Sensors"
+#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"
+
+/*****************************************************************************/
+
+#define DELAY_OUT_TIME 0x7FFFFFFF
+
+#define LIGHT_SENSOR_POLLTIME 2000000000
+
+#define SENSORS_ROTATION_VECTOR (1<<ID_RV)
+#define SENSORS_LINEAR_ACCEL (1<<ID_LA)
+#define SENSORS_GRAVITY (1<<ID_GR)
+#define SENSORS_GYROSCOPE (1<<ID_GY)
+#define SENSORS_ACCELERATION (1<<ID_A)
+#define SENSORS_MAGNETIC_FIELD (1<<ID_M)
+#define SENSORS_ORIENTATION (1<<ID_O)
+
+#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)
+
+
+#define AKM_FTRACE 0
+#define AKM_DEBUG 0
+#define AKM_DATA 0
+
+/*****************************************************************************/
+
+/* The SENSORS Module */
+static const struct sensor_t sSensorList[] = {
+ { "MPL rotation vector",
+ "Invensense",
+ 1, SENSORS_ROTATION_VECTOR_HANDLE,
+ SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 20000,{ } },
+ { "MPL linear accel",
+ "Invensense",
+ 1, SENSORS_LINEAR_ACCEL_HANDLE,
+ SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 20000,{ } },
+ { "MPL gravity",
+ "Invensense",
+ 1, SENSORS_GRAVITY_HANDLE,
+ SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 20000,{ } },
+ { "MPL Gyro",
+ "Invensense",
+ 1, SENSORS_GYROSCOPE_HANDLE,
+ SENSOR_TYPE_GYROSCOPE, 10240.0f, 1.0f, 0.5f, 20000,{ } },
+ { "MPL accel",
+ "Invensense",
+ 1, SENSORS_ACCELERATION_HANDLE,
+ SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 20000,{ } },
+ { "MPL magnetic field",
+ "Invensense",
+ 1, SENSORS_MAGNETIC_FIELD_HANDLE,
+ SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 20000,{ } },
+ { "MPL Orientation (android deprecated format)",
+ "Invensense",
+ 1, SENSORS_ORIENTATION_HANDLE,
+ SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 20000,{ } },
+
+
+};
+
+
+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 ARRAY_SIZE(sSensorList);
+}
+
+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, //all mpl entries must be consecutive and in this order
+ mpl_accel,
+ mpl_timer,
+ numSensorDrivers, // wake pipe goes here
+ mpl_power, //special handle for MPL pm interaction
+ numFds,
+ };
+
+ static const size_t wake = numFds - 2;
+ static const char WAKE_MESSAGE = 'W';
+ struct pollfd mPollFds[numFds];
+ int mWritePipeFd;
+ SensorBase* mSensors[numSensorDrivers];
+
+ int handleToDriver(int handle) const {
+ switch (handle) {
+ case ID_RV:
+ case ID_LA:
+ case ID_GR:
+ case ID_GY:
+ case ID_A:
+ case ID_M:
+ case ID_O:
+ return mpl;
+ }
+ return -EINVAL;
+ }
+};
+
+/*****************************************************************************/
+
+sensors_poll_context_t::sensors_poll_context_t()
+{
+ FUNC_LOG;
+ MPLSensor* p_mplsen = new MPLSensor();
+ setCallbackObject(p_mplsen); //setup the callback object for handing mpl callbacks
+
+ mSensors[mpl] = p_mplsen;
+ mPollFds[mpl].fd = mSensors[mpl]->getFd();
+ mPollFds[mpl].events = POLLIN;
+ mPollFds[mpl].revents = 0;
+
+ mSensors[mpl_accel] = mSensors[mpl];
+ mPollFds[mpl_accel].fd = ((MPLSensor*)mSensors[mpl])->getAccelFd();
+ mPollFds[mpl_accel].events = POLLIN;
+ mPollFds[mpl_accel].revents = 0;
+
+ mSensors[mpl_timer] = mSensors[mpl];
+ mPollFds[mpl_timer].fd = ((MPLSensor*)mSensors[mpl])->getTimerFd();
+ mPollFds[mpl_timer].events = POLLIN;
+ mPollFds[mpl_timer].revents = 0;
+
+ int wakeFds[2];
+ int result = pipe(wakeFds);
+ LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno));
+ fcntl(wakeFds[0], F_SETFL, O_NONBLOCK);
+ fcntl(wakeFds[1], F_SETFL, O_NONBLOCK);
+ mWritePipeFd = wakeFds[1];
+
+ mPollFds[wake].fd = wakeFds[0];
+ mPollFds[wake].events = POLLIN;
+ mPollFds[wake].revents = 0;
+
+ //setup MPL pm interaction handle
+ mPollFds[mpl_power].fd = ((MPLSensor*)mSensors[mpl])->getPowerFd();
+ mPollFds[mpl_power].events = POLLIN;
+ mPollFds[mpl_power].revents = 0;
+}
+
+sensors_poll_context_t::~sensors_poll_context_t()
+{
+ FUNC_LOG;
+ for (int i=0 ; i<numSensorDrivers ; i++) {
+ delete mSensors[i];
+ }
+ close(mPollFds[wake].fd);
+ close(mWritePipeFd);
+}
+
+int sensors_poll_context_t::activate(int handle, int enabled)
+{
+ FUNC_LOG;
+ int index = handleToDriver(handle);
+ if (index < 0) return index;
+ int err = mSensors[index]->enable(handle, enabled);
+ if (!err) {
+ const char wakeMessage(WAKE_MESSAGE);
+ int result = write(mWritePipeFd, &wakeMessage, 1);
+ LOGE_IF(result<0, "error sending wake message (%s)", strerror(errno));
+ }
+ return err;
+}
+
+int sensors_poll_context_t::setDelay(int handle, int64_t ns)
+{
+ FUNC_LOG;
+ int index = handleToDriver(handle);
+ if (index < 0) return index;
+ return mSensors[index]->setDelay(handle, ns);
+}
+
+int sensors_poll_context_t::pollEvents(sensors_event_t* data, int count)
+{
+ //FUNC_LOG;
+ int nbEvents = 0;
+ int n = 0;
+ int polltime = -1;
+
+ do {
+ // see if we have some leftover from the last poll()
+ for (int i=0 ; count && i<numSensorDrivers ; i++) {
+ SensorBase* const sensor(mSensors[i]);
+ if ((mPollFds[i].revents & POLLIN) || (sensor->hasPendingEvents())) {
+ int nb = sensor->readEvents(data, count);
+ if (nb < count) {
+ // no more data for this sensor
+ mPollFds[i].revents = 0;
+ }
+ count -= nb;
+ nbEvents += nb;
+ data += nb;
+
+ //special handling for the mpl, which has multiple handles
+ if(i==mpl) {
+ i+=2; //skip accel and timer
+ mPollFds[mpl_accel].revents = 0;
+ mPollFds[mpl_timer].revents = 0;
+ }
+ if(i==mpl_accel) {
+ i+=1; //skip timer
+ mPollFds[mpl_timer].revents = 0;
+ }
+ }
+ }
+
+ if (count) {
+ // we still have some room, so try to see if we can get
+ // some events immediately or just wait if we don't have
+ // anything to return
+ int i;
+
+ n = poll(mPollFds, numFds, nbEvents ? 0 : polltime);
+ if (n<0) {
+ LOGE("poll() failed (%s)", strerror(errno));
+ return -errno;
+ }
+ if (mPollFds[wake].revents & POLLIN) {
+ char msg;
+ int result = read(mPollFds[wake].fd, &msg, 1);
+ LOGE_IF(result<0, "error reading from wake pipe (%s)", strerror(errno));
+ LOGE_IF(msg != WAKE_MESSAGE, "unknown message on wake queue (0x%02x)", int(msg));
+ mPollFds[wake].revents = 0;
+ }
+ if(mPollFds[mpl_power].revents & POLLIN) {
+ ((MPLSensor*)mSensors[mpl])->handlePowerEvent();
+ mPollFds[mpl_power].revents = 0;
+ }
+ }
+ // if we have events and space, go read them
+ } while (n && count);
+
+ 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)
+{
+ FUNC_LOG;
+ 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)
+{
+ FUNC_LOG;
+ sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
+ return ctx->setDelay(handle, ns);
+}
+
+static int poll__poll(struct sensors_poll_device_t *dev,
+ sensors_event_t* data, int count)
+{
+ //FUNC_LOG;
+ 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/mlsdk/Android.mk b/mlsdk/Android.mk
new file mode 100644
index 0000000..79b8f4b
--- /dev/null
+++ b/mlsdk/Android.mk
@@ -0,0 +1,95 @@
+LOCAL_PATH := $(call my-dir)
+
+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
+LOCAL_PRELINK_MODULE := false
+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
+
+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
+LOCAL_PRELINK_MODULE := false
+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
+#LOCAL_PRELINK_MODULE := false
+#include $(BUILD_SHARED_LIBRARY)
+#include $(BUILD_MULTI_PREBUILT)
+
diff --git a/mlsdk/mllite/accel.c b/mlsdk/mllite/accel.c
new file mode 100644
index 0000000..60b8d6c
--- /dev/null
+++ b/mlsdk/mllite/accel.c
@@ -0,0 +1,189 @@
+/*
+ $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/mlsdk/mllite/accel.h b/mlsdk/mllite/accel.h
new file mode 100644
index 0000000..d3fbc6a
--- /dev/null
+++ b/mlsdk/mllite/accel.h
@@ -0,0 +1,57 @@
+/*
+ $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/mlsdk/mllite/compass.c b/mlsdk/mllite/compass.c
new file mode 100644
index 0000000..9716afd
--- /dev/null
+++ b/mlsdk/mllite/compass.c
@@ -0,0 +1,376 @@
+/*
+ $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 "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. - */
+/* -------------- */
+
+/**
+ * @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];
+ }
+ } 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;
+}
+
+/**
+ * @}
+ */
\ No newline at end of file
diff --git a/mlsdk/mllite/compass.h b/mlsdk/mllite/compass.h
new file mode 100644
index 0000000..c5dffa4
--- /dev/null
+++ b/mlsdk/mllite/compass.h
@@ -0,0 +1,60 @@
+/*
+ $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. - */
+ /* ------------ */
+
+ /* --------------- */
+ /* - Structures. - */
+ /* --------------- */
+
+ /* --------------------- */
+ /* - 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_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);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // COMPASS_H
diff --git a/mlsdk/mllite/dmpDefault.c b/mlsdk/mllite/dmpDefault.c
new file mode 100644
index 0000000..fe29376
--- /dev/null
+++ b/mlsdk/mllite/dmpDefault.c
@@ -0,0 +1,416 @@
+/*
+ $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;
+ }
+
+ {
+ 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/mlsdk/mllite/dmpDefault.h b/mlsdk/mllite/dmpDefault.h
new file mode 100644
index 0000000..0670977
--- /dev/null
+++ b/mlsdk/mllite/dmpDefault.h
@@ -0,0 +1,42 @@
+/*
+ $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/mlsdk/mllite/dmpDefaultMantis.c b/mlsdk/mllite/dmpDefaultMantis.c
new file mode 100644
index 0000000..f35aaca
--- /dev/null
+++ b/mlsdk/mllite/dmpDefaultMantis.c
@@ -0,0 +1,467 @@
+/*
+ $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/mlsdk/mllite/dmpKey.h b/mlsdk/mllite/dmpKey.h
new file mode 100644
index 0000000..f152644
--- /dev/null
+++ b/mlsdk/mllite/dmpKey.h
@@ -0,0 +1,432 @@
+/*
+ $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/mlsdk/mllite/dmpconfig.txt b/mlsdk/mllite/dmpconfig.txt
new file mode 100644
index 0000000..4643ed5
--- /dev/null
+++ b/mlsdk/mllite/dmpconfig.txt
@@ -0,0 +1,3 @@
+major version = 1
+minor version = 0
+startAddr = 0
diff --git a/mlsdk/mllite/dmpmap.h b/mlsdk/mllite/dmpmap.h
new file mode 100644
index 0000000..cb53c7c
--- /dev/null
+++ b/mlsdk/mllite/dmpmap.h
@@ -0,0 +1,276 @@
+/*
+ $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/mlsdk/mllite/invensense.h b/mlsdk/mllite/invensense.h
new file mode 100644
index 0000000..586dd25
--- /dev/null
+++ b/mlsdk/mllite/invensense.h
@@ -0,0 +1,24 @@
+/** 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/mlsdk/mllite/ml.c b/mlsdk/mllite/ml.c
new file mode 100644
index 0000000..02ba2b3
--- /dev/null
+++ b/mlsdk/mllite/ml.c
@@ -0,0 +1,1872 @@
+/*
+ $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];
+ accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii];
+ 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;
+ }
+
+ 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
+
+ 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;
+ }
+ result = inv_set_accel_calibration(accelScale, accelCal);
+ if (INV_SUCCESS != result) {
+ MPL_LOGE("Unable to set Accel Calibration\n");
+ return result;
+ }
+ 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;
+ }
+
+ 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;
+ }
+ }
+
+ if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) {
+ struct ext_slave_config config;
+ long data;
+ inv_set_fifo_rate(fifoRate);
+
+ 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/mlsdk/mllite/ml.h b/mlsdk/mllite/ml.h
new file mode 100644
index 0000000..14ba9db
--- /dev/null
+++ b/mlsdk/mllite/ml.h
@@ -0,0 +1,593 @@
+/*
+ $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
+
+/*************************************************************************/
+/* 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_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;
+
+ unsigned short flags[7];
+ 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/mlsdk/mllite/mlBiasNoMotion.c b/mlsdk/mllite/mlBiasNoMotion.c
new file mode 100644
index 0000000..73321ff
--- /dev/null
+++ b/mlsdk/mllite/mlBiasNoMotion.c
@@ -0,0 +1,393 @@
+/*
+ $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/mlsdk/mllite/mlBiasNoMotion.h b/mlsdk/mllite/mlBiasNoMotion.h
new file mode 100644
index 0000000..030dbf9
--- /dev/null
+++ b/mlsdk/mllite/mlBiasNoMotion.h
@@ -0,0 +1,40 @@
+/*
+ $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/mlsdk/mllite/mlFIFO.c b/mlsdk/mllite/mlFIFO.c
new file mode 100644
index 0000000..4270aa3
--- /dev/null
+++ b/mlsdk/mllite/mlFIFO.c
@@ -0,0 +1,2269 @@
+/*
+ $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;
+
+ if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
+
+ fifo_obj.fifo_rate = fifoRate;
+
+ 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;
+ }
+ 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)
+{
+ struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+ if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)
+ return fifo_obj.fifo_rate;
+ else
+ return (unsigned short)((1000 * fifo_obj.sample_step_size_ms) /
+ (inv_mpu_get_sampling_period_us(mldl_cfg))) - 1;
+}
+
+/**
+ * @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/mlsdk/mllite/mlFIFO.h b/mlsdk/mllite/mlFIFO.h
new file mode 100644
index 0000000..2114eb3
--- /dev/null
+++ b/mlsdk/mllite/mlFIFO.h
@@ -0,0 +1,150 @@
+/*
+ $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/mlsdk/mllite/mlFIFOHW.c b/mlsdk/mllite/mlFIFOHW.c
new file mode 100644
index 0000000..7a77e66
--- /dev/null
+++ b/mlsdk/mllite/mlFIFOHW.c
@@ -0,0 +1,328 @@
+/*
+ $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/mlsdk/mllite/mlFIFOHW.h b/mlsdk/mllite/mlFIFOHW.h
new file mode 100644
index 0000000..6f70185
--- /dev/null
+++ b/mlsdk/mllite/mlFIFOHW.h
@@ -0,0 +1,48 @@
+/*
+ $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/mlsdk/mllite/mlMathFunc.c b/mlsdk/mllite/mlMathFunc.c
new file mode 100644
index 0000000..0d8e7ec
--- /dev/null
+++ b/mlsdk/mllite/mlMathFunc.c
@@ -0,0 +1,377 @@
+/*
+ $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/mlsdk/mllite/mlMathFunc.h b/mlsdk/mllite/mlMathFunc.h
new file mode 100644
index 0000000..70fa9f4
--- /dev/null
+++ b/mlsdk/mllite/mlMathFunc.h
@@ -0,0 +1,68 @@
+/*
+ $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/mlsdk/mllite/mlSetGyroBias.c b/mlsdk/mllite/mlSetGyroBias.c
new file mode 100644
index 0000000..bd14d2e
--- /dev/null
+++ b/mlsdk/mllite/mlSetGyroBias.c
@@ -0,0 +1,198 @@
+/*
+ $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/mlsdk/mllite/mlSetGyroBias.h b/mlsdk/mllite/mlSetGyroBias.h
new file mode 100644
index 0000000..e56f18b
--- /dev/null
+++ b/mlsdk/mllite/mlSetGyroBias.h
@@ -0,0 +1,49 @@
+/*
+ $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/mlsdk/mllite/ml_mputest.c b/mlsdk/mllite/ml_mputest.c
new file mode 100644
index 0000000..d7fc608
--- /dev/null
+++ b/mlsdk/mllite/ml_mputest.c
@@ -0,0 +1,184 @@
+/*
+ $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/mlsdk/mllite/ml_mputest.h b/mlsdk/mllite/ml_mputest.h
new file mode 100644
index 0000000..705d3cc
--- /dev/null
+++ b/mlsdk/mllite/ml_mputest.h
@@ -0,0 +1,49 @@
+/*
+ $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/mlsdk/mllite/ml_stored_data.c b/mlsdk/mllite/ml_stored_data.c
new file mode 100644
index 0000000..f66fe72
--- /dev/null
+++ b/mlsdk/mllite/ml_stored_data.c
@@ -0,0 +1,1555 @@
+/*
+ $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 = 2777;
+ long bias[3];
+ int ptr = INV_CAL_HDR_LEN;
+ int i, j;
+ long long tmp;
+
+ 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_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);
+ }
+
+ 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("mlTempValid[%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]);
+ }
+
+ /* 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/mlsdk/mllite/ml_stored_data.h b/mlsdk/mllite/ml_stored_data.h
new file mode 100644
index 0000000..02634d1
--- /dev/null
+++ b/mlsdk/mllite/ml_stored_data.h
@@ -0,0 +1,62 @@
+/*
+ $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)
+#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/mlsdk/mllite/mlarray.c b/mlsdk/mllite/mlarray.c
new file mode 100644
index 0000000..fc69c44
--- /dev/null
+++ b/mlsdk/mllite/mlarray.c
@@ -0,0 +1,2524 @@
+/*
+ $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.gyro_bias[3] / 65536.0f;
+ data[1] = (float)inv_obj.gyro_bias[4] / 65536.0f;
+ data[2] = (float)inv_obj.gyro_bias[5] / 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/mlsdk/mllite/mlarray_legacy.c b/mlsdk/mllite/mlarray_legacy.c
new file mode 100644
index 0000000..9dce8f3
--- /dev/null
+++ b/mlsdk/mllite/mlarray_legacy.c
@@ -0,0 +1,587 @@
+/*
+ $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/mlsdk/mllite/mlcontrol.c b/mlsdk/mllite/mlcontrol.c
new file mode 100644
index 0000000..bd186fa
--- /dev/null
+++ b/mlsdk/mllite/mlcontrol.c
@@ -0,0 +1,797 @@
+/*
+ $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/mlsdk/mllite/mlcontrol.h b/mlsdk/mllite/mlcontrol.h
new file mode 100644
index 0000000..a834fc6
--- /dev/null
+++ b/mlsdk/mllite/mlcontrol.h
@@ -0,0 +1,217 @@
+/*
+ $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/mlsdk/mllite/mldl.c b/mlsdk/mllite/mldl.c
new file mode 100644
index 0000000..7054532
--- /dev/null
+++ b/mlsdk/mllite/mldl.c
@@ -0,0 +1,1092 @@
+/*
+ $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/mlsdk/mllite/mldl.h b/mlsdk/mllite/mldl.h
new file mode 100644
index 0000000..961d860
--- /dev/null
+++ b/mlsdk/mllite/mldl.h
@@ -0,0 +1,176 @@
+/*
+ $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/mlsdk/mllite/mldl_cfg.h b/mlsdk/mllite/mldl_cfg.h
new file mode 100644
index 0000000..b4914e2
--- /dev/null
+++ b/mlsdk/mllite/mldl_cfg.h
@@ -0,0 +1,336 @@
+/*
+ $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/mlsdk/mllite/mldl_cfg_mpu.c b/mlsdk/mllite/mldl_cfg_mpu.c
new file mode 100644
index 0000000..b03ce20
--- /dev/null
+++ b/mlsdk/mllite/mldl_cfg_mpu.c
@@ -0,0 +1,480 @@
+/*
+ $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 = %02x\n", (int)mldl_cfg->accel->suspend);
+ MPL_LOGD("slave_accel->resume = %02x\n", (int)mldl_cfg->accel->resume);
+ MPL_LOGD("slave_accel->read = %02x\n", (int)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 = %02x\n", (int)mldl_cfg->compass->suspend);
+ MPL_LOGD("slave_compass->resume = %02x\n", (int)mldl_cfg->compass->resume);
+ MPL_LOGD("slave_compass->read = %02x\n", (int)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 = %02x\n", (int)mldl_cfg->pressure->suspend);
+ MPL_LOGD("slave_pressure->resume = %02x\n", (int)mldl_cfg->pressure->resume);
+ MPL_LOGD("slave_pressure->read = %02x\n", (int)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 = %x\n",(unsigned int) 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 = %x\n",(unsigned int) 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 = %x\n",(unsigned int) 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: %d, "
+ "ext_slave_descr:%d, mpu_platform_data:%d: RamOffset: %d\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)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;
+ }
+ mpu_print_cfg(mldl_cfg);
+
+ 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;
+
+ mpu_print_cfg(mldl_cfg);
+ mldl_cfg->requested_sensors = sensors;
+ result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = ioctl((int)mlsl_handle, MPU_RESUME, NULL);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = ioctl((int)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)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = ioctl((int)mlsl_handle, MPU_SUSPEND, NULL);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ result = ioctl((int)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)gyro_handle, MPU_READ_ACCEL, data);
+ break;
+ case EXT_SLAVE_TYPE_COMPASS:
+ result = ioctl((int)gyro_handle, MPU_READ_COMPASS, data);
+ break;
+ case EXT_SLAVE_TYPE_PRESSURE:
+ result = ioctl((int)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)gyro_handle, MPU_CONFIG_ACCEL, data);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ break;
+ case EXT_SLAVE_TYPE_COMPASS:
+ result = ioctl((int)gyro_handle, MPU_CONFIG_COMPASS, data);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ break;
+ case EXT_SLAVE_TYPE_PRESSURE:
+ result = ioctl((int)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)gyro_handle, MPU_GET_CONFIG_ACCEL, data);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ break;
+ case EXT_SLAVE_TYPE_COMPASS:
+ result = ioctl((int)gyro_handle, MPU_GET_CONFIG_COMPASS, data);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ break;
+ case EXT_SLAVE_TYPE_PRESSURE:
+ result = ioctl((int)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/mlsdk/mllite/mldmp.c b/mlsdk/mllite/mldmp.c
new file mode 100644
index 0000000..200d0d1
--- /dev/null
+++ b/mlsdk/mllite/mldmp.c
@@ -0,0 +1,282 @@
+/*
+ $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;
+ }
+ 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/mlsdk/mllite/mldmp.h b/mlsdk/mllite/mldmp.h
new file mode 100644
index 0000000..f519798
--- /dev/null
+++ b/mlsdk/mllite/mldmp.h
@@ -0,0 +1,96 @@
+/*
+ $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/mlsdk/mllite/mlinclude.h b/mlsdk/mllite/mlinclude.h
new file mode 100644
index 0000000..dcbe904
--- /dev/null
+++ b/mlsdk/mllite/mlinclude.h
@@ -0,0 +1,50 @@
+/*
+ $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/mlsdk/mllite/mlstates.c b/mlsdk/mllite/mlstates.c
new file mode 100644
index 0000000..8ebb086
--- /dev/null
+++ b/mlsdk/mllite/mlstates.c
@@ -0,0 +1,269 @@
+/*
+ $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/mlsdk/mllite/mlstates.h b/mlsdk/mllite/mlstates.h
new file mode 100644
index 0000000..cbaa610
--- /dev/null
+++ b/mlsdk/mllite/mlstates.h
@@ -0,0 +1,58 @@
+/*
+ $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/mlsdk/mllite/mlsupervisor.c b/mlsdk/mllite/mlsupervisor.c
new file mode 100644
index 0000000..d5e94a3
--- /dev/null
+++ b/mlsdk/mllite/mlsupervisor.c
@@ -0,0 +1,524 @@
+/*
+ $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 int accCount = 0;
+static int compassCalStableCount = 0;
+static int compassCalCount = 0;
+
+#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;
+#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;
+
+ 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];
+ 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);
+ }
+ }
+ if (inv_obj.external_slave_callback) {
+ result = inv_obj.external_slave_callback(&inv_obj);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ }
+
+ 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.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/mlsdk/mllite/mlsupervisor.h b/mlsdk/mllite/mlsupervisor.h
new file mode 100644
index 0000000..62b427e
--- /dev/null
+++ b/mlsdk/mllite/mlsupervisor.h
@@ -0,0 +1,71 @@
+/*
+ $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/mlsdk/mllite/pressure.c b/mlsdk/mllite/pressure.c
new file mode 100644
index 0000000..9c162ec
--- /dev/null
+++ b/mlsdk/mllite/pressure.c
@@ -0,0 +1,166 @@
+/*
+ $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/mlsdk/mllite/pressure.h b/mlsdk/mllite/pressure.h
new file mode 100644
index 0000000..77c5d43
--- /dev/null
+++ b/mlsdk/mllite/pressure.h
@@ -0,0 +1,71 @@
+/*
+ $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/mlsdk/mlutils/checksum.c b/mlsdk/mlutils/checksum.c
new file mode 100644
index 0000000..a97477d
--- /dev/null
+++ b/mlsdk/mlutils/checksum.c
@@ -0,0 +1,16 @@
+#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/mlsdk/mlutils/checksum.h b/mlsdk/mlutils/checksum.h
new file mode 100644
index 0000000..4d3f046
--- /dev/null
+++ b/mlsdk/mlutils/checksum.h
@@ -0,0 +1,18 @@
+#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/mlsdk/mlutils/mputest.c b/mlsdk/mlutils/mputest.c
new file mode 100644
index 0000000..ac0fa30
--- /dev/null
+++ b/mlsdk/mlutils/mputest.c
@@ -0,0 +1,1396 @@
+/*
+ $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/mlsdk/mlutils/mputest.h b/mlsdk/mlutils/mputest.h
new file mode 100644
index 0000000..d3347c5
--- /dev/null
+++ b/mlsdk/mlutils/mputest.h
@@ -0,0 +1,54 @@
+/*
+ $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/mlsdk/mlutils/slave.h b/mlsdk/mlutils/slave.h
new file mode 100644
index 0000000..45449f6
--- /dev/null
+++ b/mlsdk/mlutils/slave.h
@@ -0,0 +1,188 @@
+/*
+ $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/mlsdk/platform/include/i2c.h b/mlsdk/platform/include/i2c.h
new file mode 100644
index 0000000..39dd255
--- /dev/null
+++ b/mlsdk/platform/include/i2c.h
@@ -0,0 +1,133 @@
+/*
+ $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/mlsdk/platform/include/linux/mpu.h b/mlsdk/platform/include/linux/mpu.h
new file mode 100644
index 0000000..af60215
--- /dev/null
+++ b/mlsdk/platform/include/linux/mpu.h
@@ -0,0 +1,332 @@
+/*
+ $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,
+
+ 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/mlsdk/platform/include/log.h b/mlsdk/platform/include/log.h
new file mode 100644
index 0000000..2c36e55
--- /dev/null
+++ b/mlsdk/platform/include/log.h
@@ -0,0 +1,344 @@
+/*
+ $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, ...) \
+ LOG(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/mlsdk/platform/include/mlmath.h b/mlsdk/platform/include/mlmath.h
new file mode 100644
index 0000000..bf54870
--- /dev/null
+++ b/mlsdk/platform/include/mlmath.h
@@ -0,0 +1,107 @@
+/*
+ $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/mlsdk/platform/include/mlos.h b/mlsdk/platform/include/mlos.h
new file mode 100644
index 0000000..78f0a83
--- /dev/null
+++ b/mlsdk/platform/include/mlos.h
@@ -0,0 +1,108 @@
+/*
+ $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__)
+typedef unsigned int HANDLE;
+#endif
+
+ /* ------------ */
+ /* - Defines. - */
+ /* ------------ */
+
+ /* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003)
+
+ /* ---------- */
+ /* - Enums. - */
+ /* ---------- */
+
+ /* --------------- */
+ /* - Structures. - */
+ /* --------------- */
+
+ /* --------------------- */
+ /* - Function p-types. - */
+ /* --------------------- */
+
+#ifndef __KERNEL__
+#include <string.h>
+ void *inv_malloc(unsigned int numBytes);
+ inv_error_t inv_free(void *ptr);
+ inv_error_t inv_create_mutex(HANDLE *mutex);
+ inv_error_t inv_lock_mutex(HANDLE mutex);
+ inv_error_t inv_unlock_mutex(HANDLE mutex);
+ FILE *inv_fopen(char *filename);
+ void inv_fclose(FILE *fp);
+
+ inv_error_t inv_destroy_mutex(HANDLE handle);
+
+ void inv_sleep(int mSecs);
+ unsigned long inv_get_tick_count(void);
+
+ /* Kernel implmentations */
+#define GFP_KERNEL (0x70)
+ static inline void *kmalloc(size_t size,
+ unsigned int gfp_flags)
+ {
+ 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/mlsdk/platform/include/mlsl.h b/mlsdk/platform/include/mlsl.h
new file mode 100644
index 0000000..535d117
--- /dev/null
+++ b/mlsdk/platform/include/mlsl.h
@@ -0,0 +1,290 @@
+/*
+ $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/mlsdk/platform/include/mltypes.h b/mlsdk/platform/include/mltypes.h
new file mode 100644
index 0000000..90a126b
--- /dev/null
+++ b/mlsdk/platform/include/mltypes.h
@@ -0,0 +1,265 @@
+/*
+ $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/mlsdk/platform/include/mpu3050.h b/mlsdk/platform/include/mpu3050.h
new file mode 100644
index 0000000..c363a00
--- /dev/null
+++ b/mlsdk/platform/include/mpu3050.h
@@ -0,0 +1,255 @@
+/*
+ $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/mlsdk/platform/include/stdint_invensense.h b/mlsdk/platform/include/stdint_invensense.h
new file mode 100644
index 0000000..d238813
--- /dev/null
+++ b/mlsdk/platform/include/stdint_invensense.h
@@ -0,0 +1,51 @@
+/*
+ $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/mlsdk/platform/linux/kernel/mpuirq.h b/mlsdk/platform/linux/kernel/mpuirq.h
new file mode 100644
index 0000000..352170b
--- /dev/null
+++ b/mlsdk/platform/linux/kernel/mpuirq.h
@@ -0,0 +1,41 @@
+/*
+ $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/mlsdk/platform/linux/kernel/slaveirq.h b/mlsdk/platform/linux/kernel/slaveirq.h
new file mode 100644
index 0000000..beb352b
--- /dev/null
+++ b/mlsdk/platform/linux/kernel/slaveirq.h
@@ -0,0 +1,35 @@
+/*
+ $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/mlsdk/platform/linux/kernel/timerirq.h b/mlsdk/platform/linux/kernel/timerirq.h
new file mode 100644
index 0000000..dc3eea2
--- /dev/null
+++ b/mlsdk/platform/linux/kernel/timerirq.h
@@ -0,0 +1,29 @@
+/*
+ $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/mlsdk/platform/linux/log_linux.c b/mlsdk/platform/linux/log_linux.c
new file mode 100644
index 0000000..8e75753
--- /dev/null
+++ b/mlsdk/platform/linux/log_linux.c
@@ -0,0 +1,114 @@
+/*
+ $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/mlsdk/platform/linux/log_printf_linux.c b/mlsdk/platform/linux/log_printf_linux.c
new file mode 100644
index 0000000..744a223
--- /dev/null
+++ b/mlsdk/platform/linux/log_printf_linux.c
@@ -0,0 +1,43 @@
+/*
+ $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/mlsdk/platform/linux/mlos_linux.c b/mlsdk/platform/linux/mlos_linux.c
new file mode 100644
index 0000000..fd3b002
--- /dev/null
+++ b/mlsdk/platform/linux/mlos_linux.c
@@ -0,0 +1,206 @@
+/*
+ $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/mlsdk/platform/linux/mlsl_linux_mpu.c b/mlsdk/platform/linux/mlsl_linux_mpu.c
new file mode 100644
index 0000000..29930b3
--- /dev/null
+++ b/mlsdk/platform/linux/mlsl_linux_mpu.c
@@ -0,0 +1,497 @@
+/*
+ $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*) open(port, O_RDWR);
+ if(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)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)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)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)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)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)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)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;
+}
+
+/**
+ * @}
+ */
+
+
