diff options
author | Ying Wang <wangying@google.com> | 2015-03-03 17:15:16 -0800 |
---|---|---|
committer | Ying Wang <wangying@google.com> | 2015-03-03 17:15:16 -0800 |
commit | f5f584ee173faef40f226c6e0e8580a2ecbe079b (patch) | |
tree | 8d23b5e09e99d77f3df8fd144c0a5ea9e36b61c5 | |
parent | fc0e95683e405f95d40284ce63d4c7c4f40af3f4 (diff) | |
download | android_hardware_invensense-f5f584ee173faef40f226c6e0e8580a2ecbe079b.tar.gz android_hardware_invensense-f5f584ee173faef40f226c6e0e8580a2ecbe079b.tar.bz2 android_hardware_invensense-f5f584ee173faef40f226c6e0e8580a2ecbe079b.zip |
Remove files for unsupported devices.
manta/grouper/mako are unsupported in MNC.
Bug: 19548232
Change-Id: Iebe443518b53e0a9ee10f4ed4ddc31ad984b9510
149 files changed, 1 insertions, 37957 deletions
diff --git a/60xx/Android.mk b/60xx/Android.mk deleted file mode 100644 index 427bbb4..0000000 --- a/60xx/Android.mk +++ /dev/null @@ -1,8 +0,0 @@ -# Can't have both manta and non-manta libsensors. -ifneq ($(filter manta, $(TARGET_DEVICE)),) -# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta. -include $(call all-named-subdir-makefiles,libsensors_iio) -else -# libsensors expects an non-IIO MPU3050. -include $(call all-named-subdir-makefiles,mlsdk libsensors) -endif diff --git a/60xx/libsensors/Android.mk b/60xx/libsensors/Android.mk deleted file mode 100644 index 326e096..0000000 --- a/60xx/libsensors/Android.mk +++ /dev/null @@ -1,47 +0,0 @@ -# Copyright (C) 2008 The Android Open Source Project -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# Modified 2011 by InvenSense, Inc - - -LOCAL_PATH := $(call my-dir) - -ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false) - -# InvenSense fragment of the HAL -include $(CLEAR_VARS) - -LOCAL_MODULE := libinvensense_hal - -LOCAL_MODULE_TAGS := optional - -LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall -LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1 - -LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp - -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include/linux -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/linux -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mllite -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mldmp -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/aichi -LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/akmd - -LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform -LOCAL_CPPFLAGS+=-DLINUX=1 -LOCAL_LDFLAGS:=-rdynamic - -include $(BUILD_SHARED_LIBRARY) - -endif diff --git a/60xx/libsensors/MPLSensor.cpp b/60xx/libsensors/MPLSensor.cpp deleted file mode 100644 index 4676d0e..0000000 --- a/60xx/libsensors/MPLSensor.cpp +++ /dev/null @@ -1,1304 +0,0 @@ -/* - * Copyright (C) 2011 Invensense, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -//#define LOG_NDEBUG 0 -//see also the EXTRA_VERBOSE define, below - -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <float.h> -#include <poll.h> -#include <unistd.h> -#include <dirent.h> -#include <stdlib.h> -#include <sys/select.h> -#include <dlfcn.h> -#include <pthread.h> - -#include <cutils/log.h> -#include <utils/KeyedVector.h> - -#include "MPLSensor.h" - -#include "math.h" -#include "ml.h" -#include "mlFIFO.h" -#include "mlsl.h" -#include "mlos.h" -#include "ml_mputest.h" -#include "ml_stored_data.h" -#include "mldl_cfg.h" -#include "mldl.h" - -#include "mpu.h" -#include "accel.h" -#include "compass.h" -#include "kernel/timerirq.h" -#include "kernel/mpuirq.h" -#include "kernel/slaveirq.h" - -extern "C" { -#include "mlsupervisor.h" -} - -#include "mlcontrol.h" -#include "sensor_params.h" - -#define EXTRA_VERBOSE (0) -//#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__) -#define FUNC_LOG -#define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) -/* this mask must turn on only the sensors that are present and managed by the MPL */ -#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) - -#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) - -/******************************************/ - -/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */ -static struct sensor_t sSensorList[] = - { { "MPL Gyroscope", "Invensense", 1, - SENSORS_GYROSCOPE_HANDLE, - SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, - { "MPL Accelerometer", "Invensense", 1, - SENSORS_ACCELERATION_HANDLE, - SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, - { "MPL Magnetic Field", "Invensense", 1, - SENSORS_MAGNETIC_FIELD_HANDLE, - SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, - { "MPL Orientation", "Invensense", 1, - SENSORS_ORIENTATION_HANDLE, - SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, { } }, - { "MPL Rotation Vector", "Invensense", 1, - SENSORS_ROTATION_VECTOR_HANDLE, - SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, - { "MPL Linear Acceleration", "Invensense", 1, - SENSORS_LINEAR_ACCEL_HANDLE, - SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, - { "MPL Gravity", "Invensense", 1, - SENSORS_GRAVITY_HANDLE, - SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, -}; - -static unsigned long long irq_timestamp = 0; -/* *************************************************************************** - * MPL interface misc. - */ -//static pointer to the object that will handle callbacks -static MPLSensor* gMPLSensor = NULL; - -/* we need to pass some callbacks to the MPL. The mpl is a C library, so - * wrappers for the C++ callback implementations are required. - */ -extern "C" { -//callback wrappers go here -void mot_cb_wrapper(uint16_t val) -{ - if (gMPLSensor) { - gMPLSensor->cbOnMotion(val); - } -} - -void procData_cb_wrapper() -{ - if(gMPLSensor) { - gMPLSensor->cbProcData(); - } -} - -} //end of extern C - -void setCallbackObject(MPLSensor* gbpt) -{ - gMPLSensor = gbpt; -} - - -/***************************************************************************** - * sensor class implementation - */ - -#define GY_ENABLED ((1<<ID_GY) & enabled_sensors) -#define A_ENABLED ((1<<ID_A) & enabled_sensors) -#define O_ENABLED ((1<<ID_O) & enabled_sensors) -#define M_ENABLED ((1<<ID_M) & enabled_sensors) -#define LA_ENABLED ((1<<ID_LA) & enabled_sensors) -#define GR_ENABLED ((1<<ID_GR) & enabled_sensors) -#define RV_ENABLED ((1<<ID_RV) & enabled_sensors) - -MPLSensor::MPLSensor() : - SensorBase(NULL, NULL), - mNewData(0), - mDmpStarted(false), - mMasterSensorMask(INV_ALL_SENSORS), - mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1), - mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false), - mUseTimerIrqAccel(false), mUsetimerIrqCompass(true), - mUseTimerirq(false), mSampleCount(0), - mEnabled(0), mPendingMask(0) -{ - FUNC_LOG; - int mpu_int_fd; - char *port = NULL; - - ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors); - - pthread_mutex_init(&mMplMutex, NULL); - - mForceSleep = false; - - /* used for identifying whether 9axis is enabled or not */ - /* this variable will be changed in initMPL() when libmpl is loaded */ - /* sensor list will be changed based on this variable */ - mNineAxisEnabled = false; - - for (size_t i = 0; i < ARRAY_SIZE(mPollFds); i++) { - mPollFds[i].fd = -1; - mPollFds[i].events = 0; - } - - pthread_mutex_lock(&mMplMutex); - - mpu_int_fd = open("/dev/mpuirq", O_RDWR); - if (mpu_int_fd == -1) { - ALOGE("could not open the mpu irq device node"); - } else { - fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK); - mIrqFds.add(MPUIRQ_FD, mpu_int_fd); - mPollFds[MPUIRQ_FD].fd = mpu_int_fd; - mPollFds[MPUIRQ_FD].events = POLLIN; - } - - accel_fd = open("/dev/accelirq", O_RDWR); - if (accel_fd == -1) { - ALOGE("could not open the accel irq device node"); - } else { - fcntl(accel_fd, F_SETFL, O_NONBLOCK); - mIrqFds.add(ACCELIRQ_FD, accel_fd); - mPollFds[ACCELIRQ_FD].fd = accel_fd; - mPollFds[ACCELIRQ_FD].events = POLLIN; - } - - timer_fd = open("/dev/timerirq", O_RDWR); - if (timer_fd == -1) { - ALOGE("could not open the timer irq device node"); - } else { - fcntl(timer_fd, F_SETFL, O_NONBLOCK); - mIrqFds.add(TIMERIRQ_FD, timer_fd); - mPollFds[TIMERIRQ_FD].fd = timer_fd; - mPollFds[TIMERIRQ_FD].events = POLLIN; - } - - data_fd = mpu_int_fd; - - if ((accel_fd == -1) && (timer_fd != -1)) { - //no accel irq and timer available - mUseTimerIrqAccel = true; - } - - memset(mPendingEvents, 0, sizeof(mPendingEvents)); - - mPendingEvents[RotationVector].version = sizeof(sensors_event_t); - mPendingEvents[RotationVector].sensor = ID_RV; - mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; - - mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); - mPendingEvents[LinearAccel].sensor = ID_LA; - mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; - - mPendingEvents[Gravity].version = sizeof(sensors_event_t); - mPendingEvents[Gravity].sensor = ID_GR; - mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; - - mPendingEvents[Gyro].version = sizeof(sensors_event_t); - mPendingEvents[Gyro].sensor = ID_GY; - mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; - - mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); - mPendingEvents[Accelerometer].sensor = ID_A; - mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; - - mPendingEvents[MagneticField].version = sizeof(sensors_event_t); - mPendingEvents[MagneticField].sensor = ID_M; - mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; - mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; - - mPendingEvents[Orientation].version = sizeof(sensors_event_t); - mPendingEvents[Orientation].sensor = ID_O; - mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; - mPendingEvents[Orientation].orientation.status - = SENSOR_STATUS_ACCURACY_HIGH; - - mHandlers[RotationVector] = &MPLSensor::rvHandler; - mHandlers[LinearAccel] = &MPLSensor::laHandler; - mHandlers[Gravity] = &MPLSensor::gravHandler; - mHandlers[Gyro] = &MPLSensor::gyroHandler; - mHandlers[Accelerometer] = &MPLSensor::accelHandler; - mHandlers[MagneticField] = &MPLSensor::compassHandler; - mHandlers[Orientation] = &MPLSensor::orienHandler; - - for (int i = 0; i < numSensors; i++) - mDelays[i] = 30000000LLU; // 30 ms by default - - if (inv_serial_start(port) != INV_SUCCESS) { - ALOGE("Fatal Error : could not open MPL serial interface"); - } - - //initialize library parameters - initMPL(); - - //setup the FIFO contents - setupFIFO(); - - - pthread_mutex_unlock(&mMplMutex); - -} - -MPLSensor::~MPLSensor() -{ - FUNC_LOG; - pthread_mutex_lock(&mMplMutex); - if (inv_dmp_stop() != INV_SUCCESS) { - ALOGW("Error: could not stop the DMP correctly.\n"); - } - - if (inv_dmp_close() != INV_SUCCESS) { - ALOGW("Error: could not close the DMP"); - } - - if (inv_serial_stop() != INV_SUCCESS) { - ALOGW("Error : could not close the serial port"); - } - pthread_mutex_unlock(&mMplMutex); - pthread_mutex_destroy(&mMplMutex); -} - -/* clear any data from our various filehandles */ -void MPLSensor::clearIrqData(bool* irq_set) -{ - unsigned int i; - int nread; - struct mpuirq_data irqdata; - - poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared - - for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { - int cur_fd = mPollFds[i].fd; - if (mPollFds[i].revents & POLLIN) { - nread = read(cur_fd, &irqdata, sizeof(irqdata)); - if (nread > 0) { - irq_set[i] = true; - irq_timestamp = irqdata.irqtime; - } - } - mPollFds[i].revents = 0; - } -} - -/* set the power states of the various sensors based on the bits set in the - * enabled_sensors parameter. - * this function modifies globalish state variables. It must be called with the mMplMutex held. */ -void MPLSensor::setPowerStates(int enabled_sensors) -{ - FUNC_LOG; - bool irq_set[5] = { false, false, false, false, false }; - - do { - - if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { - mLocalSensorMask = ALL_MPL_SENSORS_NP; - break; - } - - if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { - mLocalSensorMask = 0; - break; - } - - if (GY_ENABLED) { - mLocalSensorMask |= INV_THREE_AXIS_GYRO; - } else { - mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; - } - - if (A_ENABLED) { - mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); - } else { - mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); - } - - if (M_ENABLED) { - mLocalSensorMask |= INV_THREE_AXIS_COMPASS; - } else { - mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); - } - - } while (0); - - //record the new sensor state - inv_error_t rv; - - unsigned long sen_mask = mLocalSensorMask & mMasterSensorMask; - - bool changing_sensors = ((inv_get_dl_config()->requested_sensors - != sen_mask) && (sen_mask != 0)); - bool restart = (!mDmpStarted) && (sen_mask != 0); - - if (changing_sensors || restart) { - - ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); - - if (mDmpStarted) { - inv_dmp_stop(); - clearIrqData(irq_set); - mDmpStarted = false; - } - - if (sen_mask != inv_get_dl_config()->requested_sensors) { - rv = inv_set_mpu_sensors(sen_mask); - ALOGE_IF(rv != INV_SUCCESS, - "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", - sen_mask, rv); - } - - if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) - || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) - && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { - ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); - mUseTimerirq = true; - } else { - if (mUseTimerirq) { - ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); - clearIrqData(irq_set); - } - ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); - mUseTimerirq = false; - } - - if (!mDmpStarted) { - if (mHaveGoodMpuCal || mHaveGoodCompassCal) { - rv = inv_store_calibration(); - ALOGE_IF(rv != INV_SUCCESS, - "error: unable to store MPL calibration file"); - mHaveGoodMpuCal = false; - mHaveGoodCompassCal = false; - } - rv = inv_dmp_start(); - ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); - mDmpStarted = true; - } - } - - //check if we should stop the DMP - if (mDmpStarted && (sen_mask == 0)) { - rv = inv_dmp_stop(); - ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", - rv); - if (mUseTimerirq) { - ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); - } - clearIrqData(irq_set); - - mDmpStarted = false; - mPollTime = -1; - mCurFifoRate = -1; - } - -} - -/** - * container function for all the calls we make once to set up the MPL. - */ -void MPLSensor::initMPL() -{ - FUNC_LOG; - inv_error_t result; - unsigned short bias_update_mask = 0xFFFF; - - if (inv_dmp_open() != INV_SUCCESS) { - ALOGE("Fatal Error : could not open DMP correctly.\n"); - } - - result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work - ALOGE_IF(result != INV_SUCCESS, - "Fatal Error : could not set enabled sensors."); - - if (inv_load_calibration() != INV_SUCCESS) { - ALOGE("could not open MPL calibration file"); - } - - //check for the 9axis fusion library: if available load it and start 9x - void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); - if(h_dmp_lib) { - const char* error; - error = dlerror(); - inv_error_t (*fp_inv_enable_9x_fusion)() = - (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); - if((error = dlerror()) != NULL) { - ALOGE("%s %s", error, "inv_enable_9x_fusion"); - } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { - ALOGE( "Warning : 9 axis sensor fusion not available " - "- No compass detected.\n"); - } else { - /* 9axis is loaded and enabled */ - /* this variable is used for coming up with sensor list */ - mNineAxisEnabled = true; - } - } else { - const char* error = dlerror(); - ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); - } - - if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { - ALOGE("Error : Bias update function could not be set.\n"); - } - - if (inv_set_motion_interrupt(1) != INV_SUCCESS) { - ALOGE("Error : could not set motion interrupt"); - } - - if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { - ALOGE("Error : could not set fifo interrupt"); - } - - result = inv_set_fifo_rate(6); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); - } - - setupCallbacks(); - -} - -/** setup the fifo contents. - */ -void MPLSensor::setupFIFO() -{ - FUNC_LOG; - inv_error_t result; - - result = inv_send_accel(INV_ALL, INV_32_BIT); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_send_accel returned %d\n", result); - } - - result = inv_send_quaternion(INV_32_BIT); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_send_quaternion returned %d\n", result); - } - - result = inv_send_linear_accel(INV_ALL, INV_32_BIT); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result); - } - - result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", - result); - } - - result = inv_send_gravity(INV_ALL, INV_32_BIT); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_send_gravity returned %d\n", result); - } - - result = inv_send_gyro(INV_ALL, INV_32_BIT); - if (result != INV_SUCCESS) { - ALOGE("Fatal error: inv_send_gyro returned %d\n", result); - } - -} - -/** - * set up the callbacks that we use in all cases (outside of gestures, etc) - */ -void MPLSensor::setupCallbacks() -{ - FUNC_LOG; - if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { - ALOGE("Error : Motion callback could not be set.\n"); - - } - - if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { - ALOGE("Error : Processed data callback could not be set."); - - } -} - -/** - * handle the motion/no motion output from the MPL. - */ -void MPLSensor::cbOnMotion(uint16_t val) -{ - FUNC_LOG; - //after the first no motion, the gyro should be calibrated well - if (val == 2) { - if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { - //if gyros are on and we got a no motion, set a flag - // indicating that the cal file can be written. - mHaveGoodMpuCal = true; - } - } - - return; -} - - -void MPLSensor::cbProcData() -{ - mNewData = 1; - mSampleCount++; -} - -//these handlers transform mpl data into one of the Android sensor types -// scaling and coordinate transforms should be done in the handlers - -void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) -{ - VFUNC_LOG; - inv_error_t res; - res = inv_get_float_array(INV_GYROS, s->gyro.v); - s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; - s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; - s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; - if (res == INV_SUCCESS) - *pending_mask |= (1 << index); -} - -void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) -{ - //VFUNC_LOG; - inv_error_t res; - res = inv_get_float_array(INV_ACCELS, s->acceleration.v); - //res = inv_get_accel_float(s->acceleration.v); - s->acceleration.v[0] = s->acceleration.v[0] * 9.81; - s->acceleration.v[1] = s->acceleration.v[1] * 9.81; - s->acceleration.v[2] = s->acceleration.v[2] * 9.81; - if (res == INV_SUCCESS) - *pending_mask |= (1 << index); -} - -int MPLSensor::estimateCompassAccuracy() -{ - inv_error_t res; - int rv; - - res = inv_get_compass_accuracy(&rv); - if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) { - mHaveGoodCompassCal = true; - } - ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); - - return rv; -} - -void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) -{ - VFUNC_LOG; - inv_error_t res; - - res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); - - if (res != INV_SUCCESS) { - ALOGW( - "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", - res); - } - - s->magnetic.status = estimateCompassAccuracy(); - - if (res == INV_SUCCESS) - *pending_mask |= (1 << index); -} - -void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) -{ - VFUNC_LOG; - float quat[4]; - float norm = 0; - inv_error_t r; - - r = inv_get_float_array(INV_QUATERNION, quat); - - if (r != INV_SUCCESS) { - *pending_mask &= ~(1 << index); - return; - } else { - *pending_mask |= (1 << index); - } - - norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] - + FLT_EPSILON; - - if (norm > 1.0f) { - //renormalize - norm = sqrtf(norm); - float inv_norm = 1.0f / norm; - quat[1] = quat[1] * inv_norm; - quat[2] = quat[2] * inv_norm; - quat[3] = quat[3] * inv_norm; - } - - if (quat[0] < 0.0) { - quat[1] = -quat[1]; - quat[2] = -quat[2]; - quat[3] = -quat[3]; - } - - s->gyro.v[0] = quat[1]; - s->gyro.v[1] = quat[2]; - s->gyro.v[2] = quat[3]; - -} - -void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) -{ - VFUNC_LOG; - inv_error_t res; - res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); - s->gyro.v[0] *= 9.81; - s->gyro.v[1] *= 9.81; - s->gyro.v[2] *= 9.81; - if (res == INV_SUCCESS) - *pending_mask |= (1 << index); -} - -void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) -{ - VFUNC_LOG; - inv_error_t res; - res = inv_get_float_array(INV_GRAVITY, s->gyro.v); - s->gyro.v[0] *= 9.81; - s->gyro.v[1] *= 9.81; - s->gyro.v[2] *= 9.81; - if (res == INV_SUCCESS) - *pending_mask |= (1 << index); -} - -void MPLSensor::calcOrientationSensor(float *R, float *values) -{ - float tmp; - - //Azimuth - if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { - values[0] = (float) atan2f(-R[3], R[0]); - } else { - values[0] = (float) atan2f(R[1], R[4]); - } - values[0] *= 57.295779513082320876798154814105f; - if (values[0] < 0) { - values[0] += 360.0f; - } - //Pitch - tmp = R[7]; - if (tmp > 1.0f) - tmp = 1.0f; - if (tmp < -1.0f) - tmp = -1.0f; - values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; - if (R[8] < 0) { - values[1] = 180.0f - values[1]; - } - if (values[1] > 180.0f) { - values[1] -= 360.0f; - } - //Roll - if ((R[7] > 0.7071067f)) { - values[2] = (float) atan2f(R[6], R[7]); - } else { - values[2] = (float) atan2f(R[6], R[8]); - } - - values[2] *= 57.295779513082320876798154814105f; - if (values[2] > 90.0f) { - values[2] = 180.0f - values[2]; - } - if (values[2] < -90.0f) { - values[2] = -180.0f - values[2]; - } -} - -void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, - int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output -{ - VFUNC_LOG; - inv_error_t res; - float rot_mat[9]; - - res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); - - calcOrientationSensor(rot_mat, s->orientation.v); - - s->orientation.status = estimateCompassAccuracy(); - - if (res == INV_SUCCESS) - *pending_mask |= (1 << index); - else - ALOGW("orienHandler: data not valid (%d)", (int) res); - -} - -int MPLSensor::enable(int32_t handle, int en) -{ - FUNC_LOG; - - int what = -1; - - switch (handle) { - case ID_A: - what = Accelerometer; - break; - case ID_M: - what = MagneticField; - break; - case ID_O: - what = Orientation; - break; - case ID_GY: - what = Gyro; - break; - case ID_GR: - what = Gravity; - break; - case ID_RV: - what = RotationVector; - break; - case ID_LA: - what = LinearAccel; - break; - default: //this takes care of all the gestures - what = handle; - break; - } - - if (uint32_t(what) >= numSensors) - return -EINVAL; - - int newState = en ? 1 : 0; - int err = 0; - - pthread_mutex_lock(&mMplMutex); - if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { - short flags = newState; - mEnabled &= ~(1 << what); - mEnabled |= (uint32_t(flags) << what); - ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); - setPowerStates(mEnabled); - pthread_mutex_unlock(&mMplMutex); - if (!newState) - update_delay(); - return err; - } - pthread_mutex_unlock(&mMplMutex); - return err; -} - -int MPLSensor::setDelay(int32_t handle, int64_t ns) -{ - FUNC_LOG; - ALOGV_IF(EXTRA_VERBOSE, - " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); - int what = -1; - switch (handle) { - case ID_A: - what = Accelerometer; - break; - case ID_M: - what = MagneticField; - break; - case ID_O: - what = Orientation; - break; - case ID_GY: - what = Gyro; - break; - case ID_GR: - what = Gravity; - break; - case ID_RV: - what = RotationVector; - break; - case ID_LA: - what = LinearAccel; - break; - default: - what = handle; - break; - } - - if (uint32_t(what) >= numSensors) - return -EINVAL; - - if (ns < 0) - return -EINVAL; - - pthread_mutex_lock(&mMplMutex); - mDelays[what] = ns; - pthread_mutex_unlock(&mMplMutex); - return update_delay(); -} - -int MPLSensor::update_delay() -{ - FUNC_LOG; - int rv = 0; - bool irq_set[5]; - - pthread_mutex_lock(&mMplMutex); - - if (mEnabled) { - uint64_t wanted = -1LLU; - for (int i = 0; i < numSensors; i++) { - if (mEnabled & (1 << i)) { - uint64_t ns = mDelays[i]; - wanted = wanted < ns ? wanted : ns; - } - } - - //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns - if (wanted < 10000000LLU) { - wanted = 10000000LLU; - } - - int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 - : 0); //mpu fifo rate is in increments of 5ms - if (rate == 0) //KLP disallow fifo rate 0 - rate = 1; - - if (rate != mCurFifoRate) { - inv_error_t res; // = inv_dmp_stop(); - res = inv_set_fifo_rate(rate); - ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); - - mCurFifoRate = rate; - rv = (res == INV_SUCCESS); - } - - if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { - if (mUseTimerirq) { - ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); - clearIrqData(irq_set); - if (inv_get_dl_config()->requested_sensors - == INV_THREE_AXIS_COMPASS) { - ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, - (unsigned long) (wanted / 1000000LLU)); - ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", - (int) (wanted / 1000000LLU)); - } else { - ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, - (unsigned long) inv_get_sample_step_size_ms()); - ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", - (int) inv_get_sample_step_size_ms()); - } - } - } - - } - pthread_mutex_unlock(&mMplMutex); - return rv; -} - -/* return the current time in nanoseconds */ -int64_t MPLSensor::now_ns(void) -{ - //FUNC_LOG; - struct timespec ts; - - clock_gettime(CLOCK_MONOTONIC, &ts); - return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; -} - -int MPLSensor::readEvents(sensors_event_t* data, int count) -{ - //VFUNC_LOG; - bool irq_set[5] = { false, false, false, false, false }; - inv_error_t rv; - if (count < 1) - return -EINVAL; - int numEventReceived = 0; - - clearIrqData(irq_set); - - pthread_mutex_lock(&mMplMutex); - if (mDmpStarted) { - rv = inv_update_data(); - ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); - } - - else { - //probably just one extra read after shutting down - ALOGV_IF(EXTRA_VERBOSE, - "MPLSensor::readEvents called, but there's nothing to do."); - } - - pthread_mutex_unlock(&mMplMutex); - - if (!mNewData) { - ALOGV_IF(EXTRA_VERBOSE, "no new data"); - return 0; - } - mNewData = 0; - - /* google timestamp */ - pthread_mutex_lock(&mMplMutex); - for (int i = 0; i < numSensors; i++) { - if (mEnabled & (1 << i)) { - CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, - &mPendingMask, i); - mPendingEvents[i].timestamp = irq_timestamp; - } - } - - for (int j = 0; count && mPendingMask && j < numSensors; j++) { - if (mPendingMask & (1 << j)) { - mPendingMask &= ~(1 << j); - if (mEnabled & (1 << j)) { - *data++ = mPendingEvents[j]; - count--; - numEventReceived++; - } - } - - } - - pthread_mutex_unlock(&mMplMutex); - return numEventReceived; -} - -int MPLSensor::getFd() const -{ - return data_fd; -} - -int MPLSensor::getAccelFd() const -{ - return accel_fd; -} - -int MPLSensor::getTimerFd() const -{ - return timer_fd; -} - -int MPLSensor::getPowerFd() const -{ - int hdl = (uintptr_t) inv_get_serial_handle(); - return hdl; -} - -int MPLSensor::getPollTime() -{ - return mPollTime; -} - -bool MPLSensor::hasPendingEvents() const -{ - //if we are using the polling workaround, force the main loop to check for data every time - return (mPollTime != -1); -} - -void MPLSensor::handlePowerEvent() -{ - VFUNC_LOG; - mpuirq_data irqd; - - int fd = (uintptr_t) inv_get_serial_handle(); - read(fd, &irqd, sizeof(irqd)); - - if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { - //going to sleep - sleepEvent(); - } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { - //waking up - wakeEvent(); - } - - ioctl(fd, MPU_PM_EVENT_HANDLED, 0); -} - -void MPLSensor::sleepEvent() -{ - VFUNC_LOG; - pthread_mutex_lock(&mMplMutex); - if (mEnabled != 0) { - mForceSleep = true; - mOldEnabledMask = mEnabled; - setPowerStates(0); - } - pthread_mutex_unlock(&mMplMutex); -} - -void MPLSensor::wakeEvent() -{ - VFUNC_LOG; - pthread_mutex_lock(&mMplMutex); - if (mForceSleep) { - setPowerStates((mOldEnabledMask | mEnabled)); - } - mForceSleep = false; - pthread_mutex_unlock(&mMplMutex); -} - -/** fill in the sensor list based on which sensors are configured. - * return the number of configured sensors. - * parameter list must point to a memory region of at least 7*sizeof(sensor_t) - * parameter len gives the length of the buffer pointed to by list - */ - -int MPLSensor::populateSensorList(struct sensor_t *list, size_t len) -{ - int numsensors; - - if(len < 7*sizeof(sensor_t)) { - ALOGE("sensor list too small, not populating."); - return 0; - } - - /* fill in the base values */ - memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); - - /* first add gyro, accel and compass to the list */ - - /* fill in accel values */ - unsigned short accelId = inv_get_accel_id(); - if(accelId == 0) { - ALOGE("Can not get accel id"); - } - fillAccel(accelId, list); - - /* fill in compass values */ - unsigned short compassId = inv_get_compass_id(); - if(compassId == 0) { - ALOGE("Can not get compass id"); - } - fillCompass(compassId, list); - - /* fill in gyro values */ - fillGyro(MPU_NAME, list); - - if(mNineAxisEnabled) - { - numsensors = 7; - /* all sensors will be added to the list */ - /* fill in orientation values */ - fillOrientation(list); - - /* fill in rotation vector values */ - fillRV(list); - - /* fill in gravity values */ - fillGravity(list); - - /* fill in Linear accel values */ - fillLinearAccel(list); - } else { - /* no 9-axis sensors, zero fill that part of the list */ - numsensors = 3; - memset(list+3, 0, 4*sizeof(struct sensor_t)); - } - - return numsensors; -} - -void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list) -{ - switch (accel) { - case ACCEL_ID_LIS331: - list[Accelerometer].maxRange = ACCEL_LIS331_RANGE; - list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION; - list[Accelerometer].power = ACCEL_LIS331_POWER; - break; - - case ACCEL_ID_LIS3DH: - list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE; - list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION; - list[Accelerometer].power = ACCEL_LIS3DH_POWER; - break; - - case ACCEL_ID_KXSD9: - list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE; - list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION; - list[Accelerometer].power = ACCEL_KXSD9_POWER; - break; - - case ACCEL_ID_KXTF9: - list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE; - list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION; - list[Accelerometer].power = ACCEL_KXTF9_POWER; - break; - - case ACCEL_ID_BMA150: - list[Accelerometer].maxRange = ACCEL_BMA150_RANGE; - list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA150_POWER; - break; - - case ACCEL_ID_BMA222: - list[Accelerometer].maxRange = ACCEL_BMA222_RANGE; - list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA222_POWER; - break; - - case ACCEL_ID_BMA250: - list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; - list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA250_POWER; - break; - - case ACCEL_ID_ADXL34X: - list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE; - list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION; - list[Accelerometer].power = ACCEL_ADXL34X_POWER; - break; - - case ACCEL_ID_MMA8450: - list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; - list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; - list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; - break; - - case ACCEL_ID_MMA845X: - list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE; - list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION; - list[Accelerometer].power = ACCEL_MMA845X_POWER; - break; - - case ACCEL_ID_MPU6050: - list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; - list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; - list[Accelerometer].power = ACCEL_MPU6050_POWER; - break; - default: - ALOGE("unknown accel id -- accel params will be wrong."); - break; - } -} - -void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list) -{ - switch (compass) { - case COMPASS_ID_AK8975: - list[MagneticField].maxRange = COMPASS_AKM8975_RANGE; - list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION; - list[MagneticField].power = COMPASS_AKM8975_POWER; - break; - case COMPASS_ID_AMI30X: - list[MagneticField].maxRange = COMPASS_AMI30X_RANGE; - list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION; - list[MagneticField].power = COMPASS_AMI30X_POWER; - break; - case COMPASS_ID_AMI306: - list[MagneticField].maxRange = COMPASS_AMI306_RANGE; - list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; - list[MagneticField].power = COMPASS_AMI306_POWER; - break; - case COMPASS_ID_YAS529: - list[MagneticField].maxRange = COMPASS_YAS529_RANGE; - list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; - list[MagneticField].power = COMPASS_AMI306_POWER; - break; - case COMPASS_ID_YAS530: - list[MagneticField].maxRange = COMPASS_YAS530_RANGE; - list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; - list[MagneticField].power = COMPASS_YAS530_POWER; - break; - case COMPASS_ID_HMC5883: - list[MagneticField].maxRange = COMPASS_HMC5883_RANGE; - list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION; - list[MagneticField].power = COMPASS_HMC5883_POWER; - break; - case COMPASS_ID_MMC314X: - list[MagneticField].maxRange = COMPASS_MMC314X_RANGE; - list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION; - list[MagneticField].power = COMPASS_MMC314X_POWER; - break; - case COMPASS_ID_HSCDTD002B: - list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE; - list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION; - list[MagneticField].power = COMPASS_HSCDTD002B_POWER; - break; - case COMPASS_ID_HSCDTD004A: - list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE; - list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION; - list[MagneticField].power = COMPASS_HSCDTD004A_POWER; - break; - default: - ALOGE("unknown compass id -- compass parameters will be wrong"); - } -} - -void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) -{ - if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) { - list[Gyro].maxRange = GYRO_MPU3050_RANGE; - list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; - list[Gyro].power = GYRO_MPU3050_POWER; - } else { - list[Gyro].maxRange = GYRO_MPU6050_RANGE; - list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; - list[Gyro].power = GYRO_MPU6050_POWER; - } - return; -} - - -/* fillRV depends on values of accel and compass in the list */ -void MPLSensor::fillRV(struct sensor_t *list) -{ - /* compute power on the fly */ - list[RotationVector].power = list[Gyro].power + list[Accelerometer].power - + list[MagneticField].power; - list[RotationVector].resolution = .00001; - list[RotationVector].maxRange = 1.0; - return; -} - -void MPLSensor::fillOrientation(struct sensor_t *list) -{ - list[Orientation].power = list[Gyro].power + list[Accelerometer].power - + list[MagneticField].power; - list[Orientation].resolution = .00001; - list[Orientation].maxRange = 360.0; - return; -} - -void MPLSensor::fillGravity( struct sensor_t *list) -{ - list[Gravity].power = list[Gyro].power + list[Accelerometer].power - + list[MagneticField].power; - list[Gravity].resolution = .00001; - list[Gravity].maxRange = 9.81; - return; -} - -void MPLSensor::fillLinearAccel(struct sensor_t *list) -{ - list[Gravity].power = list[Gyro].power + list[Accelerometer].power - + list[MagneticField].power; - list[Gravity].resolution = list[Accelerometer].resolution; - list[Gravity].maxRange = list[Accelerometer].maxRange; - return; -} diff --git a/60xx/libsensors/MPLSensor.h b/60xx/libsensors/MPLSensor.h deleted file mode 100644 index f580732..0000000 --- a/60xx/libsensors/MPLSensor.h +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Copyright (C) 2011 Invensense, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -/*************Removed the gesture related info for Google check in : Meenakshi Ramamoorthi: May 31st *********/ - -#ifndef ANDROID_MPL_SENSOR_H -#define ANDROID_MPL_SENSOR_H - -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> -#include <poll.h> -#include <utils/Vector.h> -#include <utils/KeyedVector.h> -#include "sensors.h" -#include "SensorBase.h" - -/*****************************************************************************/ -/** MPLSensor implementation which fits into the HAL example for crespo provided - * * by Google. - * * WARNING: there may only be one instance of MPLSensor, ever. - */ - -class MPLSensor: public SensorBase -{ - typedef void (MPLSensor::*hfunc_t)(sensors_event_t*, uint32_t*, int); - -public: - MPLSensor(); - virtual ~MPLSensor(); - - enum - { - Gyro=0, - Accelerometer, - MagneticField, - Orientation, - RotationVector, - LinearAccel, - Gravity, - numSensors - }; - - virtual int setDelay(int32_t handle, int64_t ns); - virtual int enable(int32_t handle, int enabled); - virtual int readEvents(sensors_event_t *data, int count); - virtual int getFd() const; - virtual int getAccelFd() const; - virtual int getTimerFd() const; - virtual int getPowerFd() const; - virtual int getPollTime(); - virtual bool hasPendingEvents() const; - virtual void handlePowerEvent(); - virtual void sleepEvent(); - virtual void wakeEvent(); - int populateSensorList(struct sensor_t *list, size_t len); - void cbOnMotion(uint16_t); - void cbProcData(); - -protected: - - void clearIrqData(bool* irq_set); - void setPowerStates(int enabledsensor); - void initMPL(); - void setupFIFO(); - void setupCallbacks(); - void gyroHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void accelHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void compassHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void rvHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void laHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void gravHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void orienHandler(sensors_event_t *data, uint32_t *pendmask, int index); - void calcOrientationSensor(float *Rx, float *Val); - int estimateCompassAccuracy(); - - int mNewData; //flag indicating that the MPL calculated new output values - int mDmpStarted; - long mMasterSensorMask; - long mLocalSensorMask; - int mPollTime; - int mCurFifoRate; //current fifo rate - bool mHaveGoodMpuCal; //flag indicating that the cal file can be written - bool mHaveGoodCompassCal; - bool mUseTimerIrqAccel; - bool mUsetimerIrqCompass; - bool mUseTimerirq; - struct pollfd mPollFds[4]; - int mSampleCount; - pthread_mutex_t mMplMutex; - int64_t now_ns(); - int64_t select_ns(unsigned long long time_set[]); - - enum FILEHANDLES - { - MPUIRQ_FD, ACCELIRQ_FD, COMPASSIRQ_FD, TIMERIRQ_FD, - }; - -private: - - int update_delay(); - int accel_fd; - int timer_fd; - - uint32_t mEnabled; - uint32_t mPendingMask; - sensors_event_t mPendingEvents[numSensors]; - uint64_t mDelays[numSensors]; - hfunc_t mHandlers[numSensors]; - bool mForceSleep; - long int mOldEnabledMask; - android::KeyedVector<int, int> mIrqFds; - - /* added for dynamic get sensor list */ - bool mNineAxisEnabled; - void fillAccel(unsigned char accel, struct sensor_t *list); - void fillCompass(unsigned char compass, struct sensor_t *list); - void fillGyro(const char* gyro, struct sensor_t *list); - void fillRV(struct sensor_t *list); - void fillOrientation(struct sensor_t *list); - void fillGravity(struct sensor_t *list); - void fillLinearAccel(struct sensor_t *list); -}; - -void setCallbackObject(MPLSensor*); - -/*****************************************************************************/ - -#endif // ANDROID_MPL_SENSOR_H diff --git a/60xx/libsensors/SensorBase.cpp b/60xx/libsensors/SensorBase.cpp deleted file mode 100644 index 9cc1ee8..0000000 --- a/60xx/libsensors/SensorBase.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <string.h> -#include <unistd.h> -#include <dirent.h> -#include <sys/select.h> - -#include <cutils/log.h> - -#include <linux/input.h> - -#include "SensorBase.h" - -/*****************************************************************************/ - -SensorBase::SensorBase( - const char* dev_name, - const char* data_name) - : dev_name(dev_name), data_name(data_name), - dev_fd(-1), data_fd(-1) -{ - if (data_name) { - data_fd = openInput(data_name); - } -} - -SensorBase::~SensorBase() { - if (data_fd >= 0) { - close(data_fd); - } - if (dev_fd >= 0) { - close(dev_fd); - } -} - -int SensorBase::open_device() { - if (dev_fd<0 && dev_name) { - dev_fd = open(dev_name, O_RDONLY); - ALOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno)); - } - return 0; -} - -int SensorBase::close_device() { - if (dev_fd >= 0) { - close(dev_fd); - dev_fd = -1; - } - return 0; -} - -int SensorBase::getFd() const { - if (!data_name) { - return dev_fd; - } - return data_fd; -} - -int SensorBase::setDelay(int32_t handle, int64_t ns) { - return 0; -} - -bool SensorBase::hasPendingEvents() const { - return false; -} - -int64_t SensorBase::getTimestamp() { - struct timespec t; - t.tv_sec = t.tv_nsec = 0; - clock_gettime(CLOCK_MONOTONIC, &t); - return int64_t(t.tv_sec)*1000000000LL + t.tv_nsec; -} - -int SensorBase::openInput(const char* inputName) { - int fd = -1; - const char *dirname = "/dev/input"; - char devname[PATH_MAX]; - char *filename; - DIR *dir; - struct dirent *de; - dir = opendir(dirname); - if(dir == NULL) - return -1; - strcpy(devname, dirname); - filename = devname + strlen(devname); - *filename++ = '/'; - while((de = readdir(dir))) { - if(de->d_name[0] == '.' && - (de->d_name[1] == '\0' || - (de->d_name[1] == '.' && de->d_name[2] == '\0'))) - continue; - strcpy(filename, de->d_name); - fd = open(devname, O_RDONLY); - if (fd>=0) { - char name[80]; - if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) { - name[0] = '\0'; - } - if (!strcmp(name, inputName)) { - strcpy(input_name, filename); - break; - } else { - close(fd); - fd = -1; - } - } - } - closedir(dir); - ALOGE_IF(fd<0, "couldn't find '%s' input device", inputName); - return fd; -} diff --git a/60xx/libsensors/SensorBase.h b/60xx/libsensors/SensorBase.h deleted file mode 100644 index bb4d055..0000000 --- a/60xx/libsensors/SensorBase.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_SENSOR_BASE_H -#define ANDROID_SENSOR_BASE_H - -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> - - -/*****************************************************************************/ - -struct sensors_event_t; - -class SensorBase { -protected: - const char* dev_name; - const char* data_name; - char input_name[PATH_MAX]; - int dev_fd; - int data_fd; - - int openInput(const char* inputName); - static int64_t getTimestamp(); - - - static int64_t timevalToNano(timeval const& t) { - return t.tv_sec*1000000000LL + t.tv_usec*1000; - } - - int open_device(); - int close_device(); - -public: - SensorBase( - const char* dev_name, - const char* data_name); - - virtual ~SensorBase(); - - virtual int readEvents(sensors_event_t* data, int count) = 0; - virtual bool hasPendingEvents() const; - virtual int getFd() const; - virtual int setDelay(int32_t handle, int64_t ns); - virtual int enable(int32_t handle, int enabled) = 0; -}; - -/*****************************************************************************/ - -#endif // ANDROID_SENSOR_BASE_H diff --git a/60xx/libsensors/sensor_params.h b/60xx/libsensors/sensor_params.h deleted file mode 100644 index 4925ac4..0000000 --- a/60xx/libsensors/sensor_params.h +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright (C) 2011 Invensense, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef INV_SENSOR_PARAMS_H -#define INV_SENSOR_PARAMS_H - -/* Physical parameters of the sensors supported by Invensense MPL */ -#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV) -#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA) -#define SENSORS_GRAVITY_HANDLE (ID_GR) -#define SENSORS_GYROSCOPE_HANDLE (ID_GY) -#define SENSORS_ACCELERATION_HANDLE (ID_A) -#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M) -#define SENSORS_ORIENTATION_HANDLE (ID_O) -/******************************************/ -//COMPASS_ID_AKM -#define COMPASS_AKM8975_RANGE (9830.0f) -#define COMPASS_AKM8975_RESOLUTION (0.285f) -#define COMPASS_AKM8975_POWER (10.0f) -//COMPASS_ID_AMI30X -#define COMPASS_AMI30X_RANGE (5461.0f) -#define COMPASS_AMI30X_RESOLUTION (0.9f) -#define COMPASS_AMI30X_POWER (0.15f) -//COMPASS_ID_AMI306 -#define COMPASS_AMI306_RANGE (5461.0f) -#define COMPASS_AMI306_RESOLUTION (0.9f) -#define COMPASS_AMI306_POWER (0.15f) -//COMPASS_ID_YAS529 -#define COMPASS_YAS529_RANGE (19660.0f) -#define COMPASS_YAS529_RESOLUTION (0.012f) -#define COMPASS_YAS529_POWER (4.0f) -//COMPASS_ID_YAS530 -#define COMPASS_YAS530_RANGE (8001.0f) -#define COMPASS_YAS530_RESOLUTION (0.012f) -#define COMPASS_YAS530_POWER (4.0f) -//COMPASS_ID_HMC5883 -#define COMPASS_HMC5883_RANGE (10673.0f) -#define COMPASS_HMC5883_RESOLUTION (10.0f) -#define COMPASS_HMC5883_POWER (0.24f) -//COMPASS_ID_LSM303DLH -#define COMPASS_LSM303DLH_RANGE (10240.0f) -#define COMPASS_LSM303DLH_RESOLUTION (1.0f) -#define COMPASS_LSM303DLH_POWER (1.0f) -//COMPASS_ID_LSM303DLM -#define COMPASS_LSM303DLM_RANGE (10240.0f) -#define COMPASS_LSM303DLM_RESOLUTION (1.0f) -#define COMPASS_LSM303DLM_POWER (1.0f) -//COMPASS_ID_MMC314X -#define COMPASS_MMC314X_RANGE (400.0f) -#define COMPASS_MMC314X_RESOLUTION (2.0f) -#define COMPASS_MMC314X_POWER (0.55f) -//COMPASS_ID_HSCDTD002B -#define COMPASS_HSCDTD002B_RANGE (9830.0f) -#define COMPASS_HSCDTD002B_RESOLUTION (1.0f) -#define COMPASS_HSCDTD002B_POWER (1.0f) -//COMPASS_ID_HSCDTD004A -#define COMPASS_HSCDTD004A_RANGE (9830.0f) -#define COMPASS_HSCDTD004A_RESOLUTION (1.0f) -#define COMPASS_HSCDTD004A_POWER (1.0f) -/*******************************************/ -//ACCEL_ID_LIS331 -#define ACCEL_LIS331_RANGE (2.480f*GRAVITY_EARTH) -#define ACCEL_LIS331_RESOLUTION (.001f*GRAVITY_EARTH) -#define ACCEL_LIS331_POWER (1.0f) -//ACCEL_ID_LSM303DLX -#define ACCEL_LSM303DLX_RANGE (2.480f*GRAVITY_EARTH) -#define ACCEL_LSM303DLX_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_LSM303DLX_POWER (1.0f) -//ACCEL_ID_LIS3DH -#define ACCEL_LIS3DH_RANGE (2.480f*GRAVITY_EARTH) -#define ACCEL_LIS3DH_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_LIS3DH_POWER (1.0f) -//ACCEL_ID_KXSD9 -#define ACCEL_KXSD9_RANGE (2.5006f*GRAVITY_EARTH) -#define ACCEL_KXSD9_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_KXSD9_POWER (1.0f) -//ACCEL_ID_KXTF9 -#define ACCEL_KXTF9_RANGE (1.0f*GRAVITY_EARTH) -#define ACCEL_KXTF9_RESOLUTION (0.033f*GRAVITY_EARTH) -#define ACCEL_KXTF9_POWER (0.35f) -//ACCEL_ID_BMA150 -#define ACCEL_BMA150_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_BMA150_RESOLUTION (0.004f*GRAVITY_EARTH) -#define ACCEL_BMA150_POWER (0.2f) -//ACCEL_ID_BMA222 -#define ACCEL_BMA222_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_BMA222_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_BMA222_POWER (0.1f) -//ACCEL_ID_BMA250 -#define ACCEL_BMA250_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_BMA250_RESOLUTION (0.00391f*GRAVITY_EARTH) -#define ACCEL_BMA250_POWER (0.139f) -//ACCEL_ID_ADXL34X -#define ACCEL_ADXL34X_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_ADXL34X_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_ADXL34X_POWER (1.0f) -//ACCEL_ID_MMA8450 -#define ACCEL_MMA8450_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_MMA8450_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_MMA8450_POWER (1.0f) -//ACCEL_ID_MMA845X -#define ACCEL_MMA845X_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_MMA845X_RESOLUTION (0.001f*GRAVITY_EARTH) -#define ACCEL_MMA845X_POWER (1.0f) -//ACCEL_ID_MPU6050 -#define ACCEL_MPU6050_RANGE (2.0f*GRAVITY_EARTH) -#define ACCEL_MPU6050_RESOLUTION (0.004f*GRAVITY_EARTH) -#define ACCEL_MPU6050_POWER (0.0f) -/******************************************/ -//GYRO MPU3050 -#define RAD_P_DEG (3.14159f/180.0f) -#define GYRO_MPU3050_RANGE (2000.0f*RAD_P_DEG) -#define GYRO_MPU3050_RESOLUTION (32.8f*RAD_P_DEG) -#define GYRO_MPU3050_POWER (6.1f) -//GYRO MPU6050 -#define GYRO_MPU6050_RANGE (2000.0f*RAD_P_DEG) -#define GYRO_MPU6050_RESOLUTION (16.4f*RAD_P_DEG) -#define GYRO_MPU6050_POWER (5.5f) - -#endif - diff --git a/60xx/libsensors/sensors.h b/60xx/libsensors/sensors.h deleted file mode 100644 index 5c56da0..0000000 --- a/60xx/libsensors/sensors.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_SENSORS_H -#define ANDROID_SENSORS_H - -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> - -#include <linux/input.h> - -#include <hardware/hardware.h> -#include <hardware/sensors.h> - -__BEGIN_DECLS - -/*****************************************************************************/ - -#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) - -#define ID_MPL_BASE (0) -#define ID_GY (ID_MPL_BASE) -#define ID_A (ID_GY + 1) -#define ID_M (ID_A + 1) -#define ID_O (ID_M + 1) -#define ID_RV (ID_O + 1) -#define ID_LA (ID_RV + 1) -#define ID_GR (ID_LA + 1) - -/*****************************************************************************/ - -/* - * The SENSORS Module - */ - -__END_DECLS - -#endif // ANDROID_SENSORS_H diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk deleted file mode 100644 index fd8e472..0000000 --- a/60xx/libsensors_iio/Android.mk +++ /dev/null @@ -1,118 +0,0 @@ -# Copyright (C) 2008 The Android Open Source Project -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# Modified 2011 by InvenSense, Inc - -LOCAL_PATH := $(call my-dir) - -# InvenSense fragment of the HAL -include $(CLEAR_VARS) - -LOCAL_MODULE := libinvensense_hal - -LOCAL_MODULE_TAGS := optional -LOCAL_MODULE_OWNER := invensense - -LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall -# Comment out for ICS. Affects Android LOG macros. -LOCAL_CFLAGS += -DANDROID_JELLYBEAN - -ifeq ($(ENG_BUILD),1) -ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1) -LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL -endif -ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1) -LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL -endif -ifeq ($(COMPILE_COMPASS_YAS53x),1) -LOCAL_CFLAGS += -DCOMPASS_YAS53x -endif -ifeq ($(COMPILE_COMPASS_AK8975),1) -LOCAL_CFLAGS += -DCOMPASS_AK8975 -endif -ifeq ($(COMPILE_COMPASS_AMI306),1) -LOCAL_CFLAGS += -DCOMPASS_AMI306 -endif -else # release builds, default -LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL -endif - -LOCAL_SRC_FILES := SensorBase.cpp -LOCAL_SRC_FILES += MPLSensor.cpp -LOCAL_SRC_FILES += MPLSupport.cpp -LOCAL_SRC_FILES += InputEventReader.cpp -LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp - -LOCAL_C_INCLUDES += $(LOCAL_PATH) -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux - -LOCAL_SHARED_LIBRARIES := liblog -LOCAL_SHARED_LIBRARIES += libcutils -LOCAL_SHARED_LIBRARIES += libutils -LOCAL_SHARED_LIBRARIES += libdl -LOCAL_SHARED_LIBRARIES += libmllite - -# Additions for SysPed -LOCAL_SHARED_LIBRARIES += libmplmpu -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl -LOCAL_CPPFLAGS += -DLINUX=1 -# Experimental -ifeq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true) -LOCAL_CPPFLAGS += -DLIBMLLITE_FROM_SOURCE -endif - -include $(BUILD_SHARED_LIBRARY) - -include $(CLEAR_VARS) -LOCAL_MODULE := libmplmpu -LOCAL_SRC_FILES := libmplmpu.so -LOCAL_MODULE_TAGS := optional -LOCAL_MODULE_OWNER := invensense -LOCAL_MODULE_SUFFIX := .so -LOCAL_MODULE_CLASS := SHARED_LIBRARIES -LOCAL_MODULE_PATH := $(TARGET_OUT)/lib -OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) -LOCAL_STRIP_MODULE := true -include $(BUILD_PREBUILT) - -# Experimental -ifneq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true) -include $(CLEAR_VARS) -LOCAL_MODULE := libmllite -LOCAL_SRC_FILES := libmllite.so -LOCAL_MODULE_TAGS := optional -LOCAL_MODULE_OWNER := invensense -LOCAL_MODULE_SUFFIX := .so -LOCAL_MODULE_CLASS := SHARED_LIBRARIES -LOCAL_MODULE_PATH := $(TARGET_OUT)/lib -OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) -LOCAL_STRIP_MODULE := true -include $(BUILD_PREBUILT) -else -include $(CLEAR_VARS) -LOCAL_CFLAGS += -DANDROID_JELLYBEAN -LOCAL_CFLAGS += -DLINUX=1 -LOCAL_MODULE := libmllite -LOCAL_SRC_FILES := $(call all-c-files-under, software/core/mllite) -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include -LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux -LOCAL_MODULE_OWNER := invensense -LOCAL_MODULE_PATH := $(TARGET_OUT)/lib -LOCAL_SHARED_LIBRARIES := liblog -include $(BUILD_SHARED_LIBRARY) -endif diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp deleted file mode 100644 index be8c912..0000000 --- a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#define LOG_NDEBUG 0 - -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <unistd.h> -#include <dirent.h> -#include <sys/select.h> -#include <cutils/log.h> -#include <linux/input.h> -#include <string.h> - -#include "CompassSensor.IIO.9150.h" -#include "sensors.h" -#include "MPLSupport.h" -#include "sensor_params.h" -#include "ml_sysfs_helper.h" -#include "local_log_def.h" - -#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) - -#if defined COMPASS_YAS53x -# pragma message "Invensense compass cal with YAS53x IIO on secondary bus" -# define USE_MPL_COMPASS_HAL (1) -# define COMPASS_NAME "INV_YAS530" -#elif defined COMPASS_AK8975 -# pragma message "Invensense compass cal with AK8975 on primary bus" -# define USE_MPL_COMPASS_HAL (1) -# define COMPASS_NAME "INV_AK8975" -#elif defined INVENSENSE_COMPASS_CAL -# pragma message "Invensense compass cal with compass IIO on secondary bus" -# define USE_MPL_COMPASS_HAL (1) -# define COMPASS_NAME "INV_COMPASS" -#else -# pragma message "third party compass cal HAL" -# define USE_MPL_COMPASS_HAL (0) -// TODO: change to vendor's name -# define COMPASS_NAME "AKM8975" -#endif - - -/*****************************************************************************/ - -CompassSensor::CompassSensor() - : SensorBase(NULL, NULL), - compass_fd(-1), - mCompassTimestamp(0), - mCompassInputReader(8) -{ - VFUNC_LOG; - - if(inv_init_sysfs_attributes()) { - LOGE("Error Instantiating Compass\n"); - return; - } - - if (!strcmp(COMPASS_NAME, "INV_COMPASS")) { - mI2CBus = COMPASS_BUS_SECONDARY; - } else { - mI2CBus = COMPASS_BUS_PRIMARY; - } - - memset(mCachedCompassData, 0, sizeof(mCachedCompassData)); - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", - compassSysFs.compass_orient, getTimestamp()); - FILE *fptr; - fptr = fopen(compassSysFs.compass_orient, "r"); - if (fptr != NULL) { - int om[9]; - fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", - &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], - &om[6], &om[7], &om[8]); - fclose(fptr); - - LOGV_IF(EXTRA_VERBOSE, - "HAL:compass mounting matrix: " - "%+d %+d %+d %+d %+d %+d %+d %+d %+d", - om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); - - mCompassOrientation[0] = om[0]; - mCompassOrientation[1] = om[1]; - mCompassOrientation[2] = om[2]; - mCompassOrientation[3] = om[3]; - mCompassOrientation[4] = om[4]; - mCompassOrientation[5] = om[5]; - mCompassOrientation[6] = om[6]; - mCompassOrientation[7] = om[7]; - mCompassOrientation[8] = om[8]; - } else { - LOGE("HAL:Couldn't read compass mounting matrix"); - } - - if (!isIntegrated()) { - enable(ID_M, 0); - } -} - -CompassSensor::~CompassSensor() -{ - VFUNC_LOG; - - free(pathP); - if( compass_fd > 0) - close(compass_fd); -} - -int CompassSensor::getFd() const -{ - VHANDLER_LOG; - return compass_fd; -} - -/** - * @brief This function will enable/disable sensor. - * @param[in] handle - * which sensor to enable/disable. - * @param[in] en - * en=1, enable; - * en=0, disable - * @return if the operation is successful. - */ -int CompassSensor::enable(int32_t /*handle*/, int en) -{ - VFUNC_LOG; - - mEnable = en; - int res; - - res = write_sysfs_int(compassSysFs.compass_enable, en); - LOGE_IF(res < 0, "HAL:enable compass failed"); - - if (en) { - res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en); - res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en); - res = write_sysfs_int(compassSysFs.compass_z_fifo_enable, en); - } - - return res; -} - -int CompassSensor::setDelay(int32_t /*handle*/, int64_t ns) -{ - VFUNC_LOG; - int tempFd; - int res; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", - 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp()); - mDelay = ns; - if (ns == 0) - return -1; - tempFd = open(compassSysFs.compass_rate, O_RDWR); - res = write_attribute_sensor(tempFd, 1000000000.f / ns); - if(res < 0) { - LOGE("HAL:Compass update delay error"); - } - return res; -} - - -/** - @brief This function will return the state of the sensor. - @return 1=enabled; 0=disabled -**/ -int CompassSensor::getEnable(int32_t /*handle*/) -{ - VFUNC_LOG; - return mEnable; -} - -/* use for Invensense compass calibration */ -#define COMPASS_EVENT_DEBUG (0) -void CompassSensor::processCompassEvent(const input_event *event) -{ - VHANDLER_LOG; - - switch (event->code) { - case EVENT_TYPE_ICOMPASS_X: - LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n"); - mCachedCompassData[0] = event->value; - break; - case EVENT_TYPE_ICOMPASS_Y: - LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n"); - mCachedCompassData[1] = event->value; - break; - case EVENT_TYPE_ICOMPASS_Z: - LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n"); - mCachedCompassData[2] = event->value; - break; - } - - mCompassTimestamp = - (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L; -} - -void CompassSensor::getOrientationMatrix(signed char *orient) -{ - VFUNC_LOG; - memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation)); -} - -long CompassSensor::getSensitivity() -{ - VFUNC_LOG; - - long sensitivity; - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", - compassSysFs.compass_scale, getTimestamp()); - inv_read_data(compassSysFs.compass_scale, &sensitivity); - return sensitivity; -} - -/** - @brief This function is called by sensors_mpl.cpp - to read sensor data from the driver. - @param[out] data sensor data is stored in this variable. Scaled such that - 1 uT = 2^16 - @para[in] timestamp data's timestamp - @return 1, if 1 sample read, 0, if not, negative if error - */ -int CompassSensor::readSample(long *data, int64_t *timestamp) -{ - VHANDLER_LOG; - - int done = 0; - - ssize_t n = mCompassInputReader.fill(compass_fd); - if (n < 0) { - LOGE("HAL:no compass events read"); - return n; - } - - input_event const* event; - - while (done == 0 && mCompassInputReader.readEvent(&event)) { - int type = event->type; - if (type == EV_REL) { - processCompassEvent(event); - } else if (type == EV_SYN) { - *timestamp = mCompassTimestamp; - memcpy(data, mCachedCompassData, sizeof(mCachedCompassData)); - done = 1; - } else { - LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mCompassInputReader.next(); - } - - return done; -} - -/** - * @brief This function will return the current delay for this sensor. - * @return delay in nanoseconds. - */ -int64_t CompassSensor::getDelay(int32_t /*handle*/) -{ - VFUNC_LOG; - return mDelay; -} - -void CompassSensor::fillList(struct sensor_t *list) -{ - VFUNC_LOG; - - const char *compass = COMPASS_NAME; - - if (compass) { - if(!strcmp(compass, "INV_COMPASS")) { - list->maxRange = COMPASS_MPU9150_RANGE; - /* since target platform would use AK8963 instead of AK8975, - need to adopt AK8963's resolution here */ - list->resolution = COMPASS_AKM8963_RESOLUTION; - list->power = COMPASS_MPU9150_POWER; - list->minDelay = COMPASS_MPU9150_MINDELAY; - return; - } - if(!strcmp(compass, "compass") - || !strcmp(compass, "INV_AK8975") ) { - list->maxRange = COMPASS_AKM8975_RANGE; - list->resolution = COMPASS_AKM8975_RESOLUTION; - list->power = COMPASS_AKM8975_POWER; - list->minDelay = COMPASS_AKM8975_MINDELAY; - return; - } - if(!strcmp(compass, "INV_YAS530")) { - list->maxRange = COMPASS_YAS53x_RANGE; - list->resolution = COMPASS_YAS53x_RESOLUTION; - list->power = COMPASS_YAS53x_POWER; - list->minDelay = COMPASS_YAS53x_MINDELAY; - return; - } - if(!strcmp(compass, "INV_AMI306")) { - list->maxRange = COMPASS_AMI306_RANGE; - list->resolution = COMPASS_AMI306_RESOLUTION; - list->power = COMPASS_AMI306_POWER; - list->minDelay = COMPASS_AMI306_MINDELAY; - return; - } - } - - LOGE("HAL:unknown compass id %s -- " - "params default to ak8975 and might be wrong.", - compass); - list->maxRange = COMPASS_AKM8975_RANGE; - list->resolution = COMPASS_AKM8975_RESOLUTION; - list->power = COMPASS_AKM8975_POWER; - list->minDelay = COMPASS_AKM8975_MINDELAY; -} - -int CompassSensor::inv_init_sysfs_attributes(void) -{ - VFUNC_LOG; - - unsigned char i = 0; - char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN]; - char *sptr; - char **dptr; - - pathP = (char*)malloc( - sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); - sptr = pathP; - dptr = (char**)&compassSysFs; - if (sptr == NULL) - return -1; - - do { - *dptr++ = sptr; - sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); - } while (++i < COMPASS_MAX_SYSFS_ATTRB); - - // get proper (in absolute/relative) IIO path & build MPU's sysfs paths - // inv_get_sysfs_abs_path(sysfs_path); - if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) { - ALOGE("CompassSensor failed get sysfs path"); - return -1; - } - - inv_get_iio_trigger_path(iio_trigger_path); - -#if defined COMPASS_AK8975 - char tbuf[2]; - int num; - inv_get_input_number(COMPASS_NAME, &num); - tbuf[0] = num + 0x30; - tbuf[1] = 0; - sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf); - strcat(sysfs_path, "/ak8975"); - - sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable"); - sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate"); - sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale"); - sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); -#else - sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable"); - sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en"); - sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en"); - sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en"); - sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency"); - sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale"); - sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); -#endif - -#if SYSFS_VERBOSE - // test print sysfs paths - dptr = (char**)&compassSysFs; - for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) { - LOGE("HAL:sysfs path: %s", *dptr++); - } -#endif - return 0; -} diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.h b/60xx/libsensors_iio/CompassSensor.IIO.9150.h deleted file mode 100644 index 6e9bf03..0000000 --- a/60xx/libsensors_iio/CompassSensor.IIO.9150.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef COMPASS_SENSOR_H -#define COMPASS_SENSOR_H - -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> - -// TODO fixme, need input_event -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> -#include <poll.h> -#include <utils/Vector.h> -#include <utils/KeyedVector.h> - -#include "sensors.h" -#include "SensorBase.h" -#include "InputEventReader.h" - -class CompassSensor : public SensorBase { - -public: - CompassSensor(); - virtual ~CompassSensor(); - - virtual int getFd() const; - virtual int enable(int32_t handle, int enabled); - virtual int setDelay(int32_t handle, int64_t ns); - virtual int getEnable(int32_t handle); - virtual int64_t getDelay(int32_t handle); - - // unnecessary for MPL - virtual int readEvents(sensors_event_t* /*data*/, int /*count*/) { return 0; } - - int readSample(long *data, int64_t *timestamp); - int providesCalibration() { return 0; } - void getOrientationMatrix(signed char *orient); - long getSensitivity(); - int getAccuracy() { return 0; } - void fillList(struct sensor_t *list); - int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); } - -private: - enum CompassBus { - COMPASS_BUS_PRIMARY = 0, - COMPASS_BUS_SECONDARY = 1 - } mI2CBus; - - struct sysfs_attrbs { - char *compass_enable; - char *compass_x_fifo_enable; - char *compass_y_fifo_enable; - char *compass_z_fifo_enable; - char *compass_rate; - char *compass_scale; - char *compass_orient; - } compassSysFs; - - // implementation specific - signed char mCompassOrientation[9]; - long mCachedCompassData[3]; - int compass_fd; - int64_t mCompassTimestamp; - InputEventCircularReader mCompassInputReader; - int64_t mDelay; - int mEnable; - char *pathP; - - void processCompassEvent(const input_event *event); - int inv_init_sysfs_attributes(void); -}; - -/*****************************************************************************/ - -#endif // COMPASS_SENSOR_H diff --git a/60xx/libsensors_iio/InputEventReader.cpp b/60xx/libsensors_iio/InputEventReader.cpp deleted file mode 100644 index fc48892..0000000 --- a/60xx/libsensors_iio/InputEventReader.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define LOG_NDEBUG 0
-
-#include <stdint.h>
-#include <errno.h>
-#include <string.h>
-#include <unistd.h>
-#include <poll.h>
-
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <cutils/log.h>
-
-#include "InputEventReader.h"
-#include "local_log_def.h"
-
-/*****************************************************************************/
-
-struct input_event;
-
-InputEventCircularReader::InputEventCircularReader(size_t numEvents)
- : mBuffer(new input_event[numEvents * 2]),
- mBufferEnd(mBuffer + numEvents),
- mHead(mBuffer),
- mCurr(mBuffer),
- mFreeSpace(numEvents)
-{
-}
-
-InputEventCircularReader::~InputEventCircularReader()
-{
- delete [] mBuffer;
-}
-
-#define INPUT_EVENT_DEBUG (0)
-ssize_t InputEventCircularReader::fill(int fd)
-{
- size_t numEventsRead = 0;
- LOGV_IF(INPUT_EVENT_DEBUG,
- "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd);
- if (mFreeSpace) {
- const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event));
- if (nread < 0 || nread % sizeof(input_event)) {
- //LOGE("Partial event received nread=%d, required=%d",
- // nread, sizeof(input_event));
- //LOGE("FD trying to read is: %d");
- // we got a partial event!!
- if (INPUT_EVENT_DEBUG) {
- LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n",
- __PRETTY_FUNCTION__);
- LOGV_IF(nread % sizeof(input_event),
- "DEBUG:%s exit nread %% sizeof(input_event)\n",
- __PRETTY_FUNCTION__);
- }
- return (nread < 0 ? -errno : -EINVAL);
- }
-
- numEventsRead = nread / sizeof(input_event);
- if (numEventsRead) {
- mHead += numEventsRead;
- mFreeSpace -= numEventsRead;
- if (mHead > mBufferEnd) {
- size_t s = mHead - mBufferEnd;
- memcpy(mBuffer, mBufferEnd, s * sizeof(input_event));
- mHead = mBuffer + s;
- }
- }
- }
-
- LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__);
- return numEventsRead;
-}
-
-ssize_t InputEventCircularReader::readEvent(input_event const** events)
-{
- *events = mCurr;
- ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace;
- return available ? 1 : 0;
-}
-
-void InputEventCircularReader::next()
-{
- mCurr++;
- mFreeSpace++;
- if (mCurr >= mBufferEnd) {
- mCurr = mBuffer;
- }
-}
diff --git a/60xx/libsensors_iio/InputEventReader.h b/60xx/libsensors_iio/InputEventReader.h deleted file mode 100644 index 5c752db..0000000 --- a/60xx/libsensors_iio/InputEventReader.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -* Copyright (C) 2012 Invensense, Inc. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*/ - -#ifndef ANDROID_INPUT_EVENT_READER_H -#define ANDROID_INPUT_EVENT_READER_H - -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> - -#include "SensorBase.h" - -/*****************************************************************************/ - -struct input_event; - -class InputEventCircularReader -{ - struct input_event* const mBuffer; - struct input_event* const mBufferEnd; - struct input_event* mHead; - struct input_event* mCurr; - ssize_t mFreeSpace; - -public: - InputEventCircularReader(size_t numEvents); - ~InputEventCircularReader(); - ssize_t fill(int fd); - ssize_t readEvent(input_event const** events); - void next(); -}; - -/*****************************************************************************/ - -#endif // ANDROID_INPUT_EVENT_READER_H diff --git a/60xx/libsensors_iio/License.txt b/60xx/libsensors_iio/License.txt deleted file mode 100644 index 930f931..0000000 --- a/60xx/libsensors_iio/License.txt +++ /dev/null @@ -1,217 +0,0 @@ -SOFTWARE LICENSE AGREEMENT - -Unless you and InvenSense Corporation ("InvenSense") execute a separate written -software license agreement governing use of the accompanying software, this -software is licensed to you under the terms of this Software License -Agreement ("Agreement"). - -ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR -ACCEPTANCE OF THIS AGREEMENT. - -1. DEFINITIONS. - -1.1. "InvenSense Product" means any of the proprietary integrated circuit -product(s) sold by InvenSense with which the Software was designed to be used, -or their successors. - -1.2. "Licensee" means you or if you are accepting on behalf of an entity -then the entity and its affiliates exercising rights under, and complying -with all of the terms of this Agreement. - -1.3. "Software" shall mean that software made available by InvenSense to -Licensee in binary code form with this Agreement. - -2. LICENSE GRANT; OWNERSHIP - -2.1. License Grants. Subject to the terms and conditions of this Agreement, -InvenSense hereby grants to Licensee a non-exclusive, non-transferable, -royalty-free license (i) to use and integrate the Software in conjunction -with any other software; and (ii) to reproduce and distribute the Software -complete, unmodified and only for use with a InvenSense Product. - -2.2. Restriction on Modification. If and to the extent that the Software is -designed to be compliant with any published communications standard -(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards), -Licensee may not make any modifications to the Software that would cause the -Software or the accompanying InvenSense Products to be incompatible with such -standard. - -2.3. Restriction on Distribution. Licensee shall only distribute the -Software (a) under the terms of this Agreement and a copy of this Agreement -accompanies such distribution, and (b) agrees to defend and indemnify -InvenSense and its licensors from and against any damages, costs, liabilities, -settlement amounts and/or expenses (including attorneys' fees) incurred in -connection with any claim, lawsuit or action by any third party that arises -or results from the use or distribution of any and all Software by the -Licensee except as contemplated herein. - -2.4. Proprietary Notices. Licensee shall not remove, efface or obscure any -copyright or trademark notices from the Software. Licensee shall include -reproductions of the InvenSense copyright notice with each copy of the -Software, except where such Software is embedded in a manner not readily -accessible to the end user. Licensee acknowledges that any symbols, -trademarks, tradenames, and service marks adopted by InvenSense to identify the -Software belong to InvenSense and that Licensee shall have no rights therein. - -2.5. Ownership. InvenSense shall retain all right, title and interest, -including all intellectual property rights, in and to the Software. Licensee -hereby covenants that it will not assert any claim that the Software created -by or for InvenSense infringe any intellectual property right owned or -controlled by Licensee. - -2.6. No Other Rights Granted; Restrictions. Apart from the license rights -expressly set forth in this Agreement, InvenSense does not grant and Licensee -does not receive any ownership right, title or interest nor any security -interest or other interest in any intellectual property rights relating to -the Software, nor in any copy of any part of the foregoing. No license is -granted to Licensee in any human readable code of the Software (source code). -Licensee shall not (i) use, license, sell or otherwise distribute the -Software except as provided in this Agreement, (ii) attempt to reverse -engineer, decompile or disassemble any portion of the Software; or (iii) use -the Software or other material in violation of any applicable law or -regulation, including but not limited to any regulatory agency, such as FCC, -rules. - -3. NO WARRANTY OR SUPPORT - -3.1. No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND -LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE, -COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE. INVENSENSE SPECIFICALLY -DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC -PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR -DOCUMENTATION FOR THE SOFTWARE. WITHOUT LIMITATION OF THE ABOVE, INVENSENSE -GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT -INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS -THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR -RELIABILITY. - -3.2. No Support. Nothing in this agreement shall obligate InvenSense to -provide any support for the Software. InvenSense may, but shall be under no -obligation to, correct any defects in the Software and/or provide updates to -licensees of the Software. Licensee shall make reasonable efforts to -promptly report to InvenSense any defects it finds in the Software, as an aid -to creating improved revisions of the Software. - -3.3. Dangerous Applications. The Software is not designed, intended, or -certified for use in components of systems intended for the operation of -weapons, weapons systems, nuclear installations, means of mass -transportation, aviation, life-support computers or equipment (including -resuscitation equipment and surgical implants), pollution control, hazardous -substances management, or for any other dangerous application in which the -failure of the Software could create a situation where personal injury or -death may occur. Licensee understands that use of the Software in such -applications is fully at the risk of Licensee. - -4. TERM AND TERMINATION - -4.1. Termination. This Agreement will automatically terminate if Licensee -fails to comply with any of the terms and conditions hereof. In such event, -Licensee must destroy all copies of the Software and all of its component -parts. - -4.2. Effect Of Termination. Upon any termination of this Agreement, the -rights and licenses granted to Licensee under this Agreement shall -immediately terminate. - -4.3. Survival. The rights and obligations under this Agreement which by -their nature should survive termination will remain in effect after -expiration or termination of this Agreement. - -5. CONFIDENTIALITY - -5.1. Obligations. Licensee acknowledges and agrees that any documentation -relating to the Software, and any other information (if such other -information is identified as confidential or should be recognized as -confidential under the circumstances) provided to Licensee by InvenSense -hereunder (collectively, "Confidential Information") constitute the -confidential and proprietary information of InvenSense, and that Licensee's -protection thereof is an essential condition to Licensee's use and possession -of the Software. Licensee shall retain all Confidential Information in -strict confidence and not disclose it to any third party or use it in any way -except under a written agreement with terms and conditions at least as -protective as the terms of this Section. Licensee will exercise at least the -same amount of diligence in preserving the secrecy of the Confidential -Information as it uses in preserving the secrecy of its own most valuable -confidential information, but in no event less than reasonable diligence. -Information shall not be considered Confidential Information if and to the -extent that it: (i) was in the public domain at the time it was disclosed or -has entered the public domain through no fault of Licensee; (ii) was known to -Licensee, without restriction, at the time of disclosure as proven by the -files of Licensee in existence at the time of disclosure; or (iii) becomes -known to Licensee, without restriction, from a source other than InvenSense -without breach of this Agreement by Licensee and otherwise not in violation -of InvenSense's rights. - -5.2. Return of Confidential Information. Notwithstanding the foregoing, all -documents and other tangible objects containing or representing InvenSense -Confidential Information and all copies thereof which are in the possession -of Licensee shall be and remain the property of InvenSense, and shall be -promptly returned to InvenSense upon written request by InvenSense or upon -termination of this Agreement. - -6. LIMITATION OF LIABILITY -TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF -INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL, -SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR -OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS -OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH -DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT -(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR -SOFTWARE UNDER THIS AGREEMENT. THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING -ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY. - -7. MISCELLANEOUS - -7.1. Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS -SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND -REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE -OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS. -WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE -TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED. - -7.2 Assignment. This Agreement shall be binding upon and inure to the -benefit of the parties and their respective successors and assigns, provided, -however that Licensee may not assign this Agreement or any rights or -obligation hereunder, directly or indirectly, by operation of law or -otherwise, without the prior written consent of InvenSense, and any such -attempted assignment shall be void. Notwithstanding the foregoing, Licensee -may assign this Agreement to a successor to all or substantially all of its -business or assets to which this Agreement relates that is not a competitor -of InvenSense. - -7.3. Governing Law; Venue. This Agreement shall be governed by the laws of -California without regard to any conflict-of-laws rules, and the United -Nations Convention on Contracts for the International Sale of Goods is hereby -excluded. The sole jurisdiction and venue for actions related to the subject -matter hereof shall be the state and federal courts located in the County of -Orange, California, and both parties hereby consent to such jurisdiction and -venue. - -7.4. Severability. All terms and provisions of this Agreement shall, if -possible, be construed in a manner which makes them valid, but in the event -any term or provision of this Agreement is found by a court of competent -jurisdiction to be illegal or unenforceable, the validity or enforceability -of the remainder of this Agreement shall not be affected if the illegal or -unenforceable provision does not materially affect the intent of this -Agreement. If the illegal or unenforceable provision materially affects the -intent of the parties to this Agreement, this Agreement shall become -terminated. - -7.5. Equitable Relief. Licensee hereby acknowledges that its breach of this -Agreement would cause irreparable harm and significant injury to InvenSense -that may be difficult to ascertain and that a remedy at law would be -inadequate. Accordingly, Licensee agrees that InvenSense shall have the right -to seek and obtain immediate injunctive relief to enforce obligations under -the Agreement in addition to any other rights and remedies it may have. - -7.6. Waiver. The waiver of, or failure to enforce, any breach or default -hereunder shall not constitute the waiver of any other or subsequent breach -or default. - -7.7. Entire Agreement. This Agreement sets forth the entire Agreement -between the parties and supersedes any and all prior proposals, agreements -and representations between them, whether written or oral concerning the -Software. This Agreement may be changed only by mutual agreement of the -parties in writing. - diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp deleted file mode 100644 index 031ae6e..0000000 --- a/60xx/libsensors_iio/MPLSensor.cpp +++ /dev/null @@ -1,2764 +0,0 @@ -/* -* Copyright (C) 2012 Invensense, Inc. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*/ - -#define LOG_NDEBUG 0 -//see also the EXTRA_VERBOSE define in the MPLSensor.h header file - -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <float.h> -#include <poll.h> -#include <unistd.h> -#include <dirent.h> -#include <stdlib.h> -#include <sys/select.h> -#include <sys/syscall.h> -#include <dlfcn.h> -#include <pthread.h> -#include <cutils/log.h> -#include <utils/KeyedVector.h> -#include <utils/String8.h> -#include <string.h> -#include <linux/input.h> -#include <utils/Atomic.h> - -#include "MPLSensor.h" -#include "MPLSupport.h" -#include "sensor_params.h" -#include "local_log_def.h" - -#include "invensense.h" -#include "invensense_adv.h" -#include "ml_stored_data.h" -#include "ml_load_dmp.h" -#include "ml_sysfs_helper.h" - -// #define TESTING - -#ifdef THIRD_PARTY_ACCEL -# warning "Third party accel" -# define USE_THIRD_PARTY_ACCEL (1) -#else -# define USE_THIRD_PARTY_ACCEL (0) -#endif - -#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) - -/******************************************************************************/ -/* MPL interface misc. */ -/******************************************************************************/ -static int hertz_request = 200; - -#define DEFAULT_MPL_GYRO_RATE (20000L) //us -#define DEFAULT_MPL_COMPASS_RATE (20000L) //us - -#define DEFAULT_HW_GYRO_RATE (100) //Hz -#define DEFAULT_HW_ACCEL_RATE (20) //ms -#define DEFAULT_HW_COMPASS_RATE (20000000L) //ns -#define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns - -/* convert ns to hardware units */ -#define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz -#define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms -#define HW_COMPASS_RATE_NS (rate_request) // to ns - -/* convert Hz to hardware units */ -#define HW_GYRO_RATE_HZ (hertz_request) -#define HW_ACCEL_RATE_HZ (1000 / hertz_request) -#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request) - -#define RATE_200HZ 5000000LL -#define RATE_15HZ 66667000LL -#define RATE_5HZ 200000000LL - -static struct sensor_t sSensorList[] = -{ - {"MPL Gyroscope", "Invensense", 1, - SENSORS_GYROSCOPE_HANDLE, - SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Raw Gyroscope", "Invensense", 1, - SENSORS_RAW_GYROSCOPE_HANDLE, - SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Accelerometer", "Invensense", 1, - SENSORS_ACCELERATION_HANDLE, - SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Magnetic Field", "Invensense", 1, - SENSORS_MAGNETIC_FIELD_HANDLE, - SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Orientation", "Invensense", 1, - SENSORS_ORIENTATION_HANDLE, - SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Rotation Vector", "Invensense", 1, - SENSORS_ROTATION_VECTOR_HANDLE, - SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Linear Acceleration", "Invensense", 1, - SENSORS_LINEAR_ACCEL_HANDLE, - SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Gravity", "Invensense", 1, - SENSORS_GRAVITY_HANDLE, - SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - -#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION - {"MPL Screen Orientation", "Invensense ", 1, - SENSORS_SCREEN_ORIENTATION_HANDLE, - SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}}, -#endif -}; - -MPLSensor *MPLSensor::gMPLSensor = NULL; - -extern "C" { -void procData_cb_wrapper() -{ - if(MPLSensor::gMPLSensor) { - MPLSensor::gMPLSensor->cbProcData(); - } -} - -void setCallbackObject(MPLSensor* gbpt) -{ - MPLSensor::gMPLSensor = gbpt; -} - -MPLSensor* getCallbackObject() { - return MPLSensor::gMPLSensor; -} - -} // end of extern C - -#ifdef INV_PLAYBACK_DBG -static FILE *logfile = NULL; -#endif - -pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER; - -/******************************************************************************* - * MPLSensor class implementation - ******************************************************************************/ - -MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *)) - : SensorBase(NULL, NULL), - mNewData(0), - mMasterSensorMask(INV_ALL_SENSORS), - mLocalSensorMask(0), - mPollTime(-1), - mHaveGoodMpuCal(0), - mGyroAccuracy(0), - mAccelAccuracy(0), - mCompassAccuracy(0), - mSampleCount(0), - dmp_orient_fd(-1), - mDmpOrientationEnabled(0), - mEnabled(0), - mOldEnabledMask(0), - mAccelInputReader(4), - mGyroInputReader(32), - mTempScale(0), - mTempOffset(0), - mTempCurrentTime(0), - mAccelScale(2), - mPendingMask(0), - mSensorMask(0), - mFeatureActiveMask(0) { - VFUNC_LOG; - - inv_error_t rv; - int fd; - char *ver_str; - - mCompassSensor = compass; - - LOGV_IF(EXTRA_VERBOSE, - "HAL:MPLSensor constructor : numSensors = %d", numSensors); - - pthread_mutex_init(&mMplMutex, NULL); - pthread_mutex_init(&mHALMutex, NULL); - memset(mGyroOrientation, 0, sizeof(mGyroOrientation)); - memset(mAccelOrientation, 0, sizeof(mAccelOrientation)); - -#ifdef INV_PLAYBACK_DBG - LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); - logfile = fopen("/data/playback.bin", "wb"); - if (logfile) - inv_turn_on_data_logging(logfile); -#endif - - /* setup sysfs paths */ - inv_init_sysfs_attributes(); - - /* get chip name */ - if (inv_get_chip_name(chip_ID) != INV_SUCCESS) { - LOGE("HAL:ERR- Failed to get chip ID\n"); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID); - } - - enable_iio_sysfs(); - - /* turn on power state */ - onPower(1); - - /* reset driver master enable */ - masterEnable(0); - - if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) { - /* Load DMP image if capable, ie. MPU6xxx/9xxx */ - loadDMP(); - } - - /* open temperature fd for temp comp */ - LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature); - gyro_temperature_fd = open(mpu.temperature, O_RDONLY); - if (gyro_temperature_fd == -1) { - LOGE("HAL:could not open temperature node"); - } else { - LOGV_IF(EXTRA_VERBOSE, - "HAL:temperature_fd opened: %s", mpu.temperature); - } - - /* read accel FSR to calcuate accel scale later */ - if (!USE_THIRD_PARTY_ACCEL) { - char buf[3]; - int count = 0; - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp()); - - fd = open(mpu.accel_fsr, O_RDONLY); - if(fd < 0) { - LOGE("HAL:Error opening accel FSR"); - } else { - memset(buf, 0, sizeof(buf)); - count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 1) { - LOGE("HAL:Error reading accel FSR"); - } else { - count = sscanf(buf, "%d", &mAccelScale); - if(count) - LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale); - } - close(fd); - } - } - - /* initialize sensor data */ - memset(mPendingEvents, 0, sizeof(mPendingEvents)); - - mPendingEvents[RotationVector].version = sizeof(sensors_event_t); - mPendingEvents[RotationVector].sensor = ID_RV; - mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; - - mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); - mPendingEvents[LinearAccel].sensor = ID_LA; - mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; - - mPendingEvents[Gravity].version = sizeof(sensors_event_t); - mPendingEvents[Gravity].sensor = ID_GR; - mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; - - mPendingEvents[Gyro].version = sizeof(sensors_event_t); - mPendingEvents[Gyro].sensor = ID_GY; - mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; - - mPendingEvents[RawGyro].version = sizeof(sensors_event_t); - mPendingEvents[RawGyro].sensor = ID_RG; - mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE; - - mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); - mPendingEvents[Accelerometer].sensor = ID_A; - mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; - - /* Invensense compass calibration */ - mPendingEvents[MagneticField].version = sizeof(sensors_event_t); - mPendingEvents[MagneticField].sensor = ID_M; - mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; - mPendingEvents[MagneticField].magnetic.status = - SENSOR_STATUS_ACCURACY_HIGH; - - mPendingEvents[Orientation].version = sizeof(sensors_event_t); - mPendingEvents[Orientation].sensor = ID_O; - mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; - mPendingEvents[Orientation].orientation.status - = SENSOR_STATUS_ACCURACY_HIGH; - - mHandlers[RotationVector] = &MPLSensor::rvHandler; - mHandlers[LinearAccel] = &MPLSensor::laHandler; - mHandlers[Gravity] = &MPLSensor::gravHandler; - mHandlers[Gyro] = &MPLSensor::gyroHandler; - mHandlers[RawGyro] = &MPLSensor::rawGyroHandler; - mHandlers[Accelerometer] = &MPLSensor::accelHandler; - mHandlers[MagneticField] = &MPLSensor::compassHandler; - mHandlers[Orientation] = &MPLSensor::orienHandler; - - for (int i = 0; i < numSensors; i++) { - mDelays[i] = 0; - } - - (void)inv_get_version(&ver_str); - LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str); - - /* setup MPL */ - inv_constructor_init(); - - /* load calibration file from /data/inv_cal_data.bin */ - rv = inv_load_calibration(); - if(rv == INV_SUCCESS) - LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded"); - else - LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); - - /* Takes external Accel Calibration Load Method */ - if( m_pt2AccelCalLoadFunc != NULL) - { - long accel_offset[3]; - long tmp_offset[3]; - int result = m_pt2AccelCalLoadFunc(accel_offset); - if(result) - LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result); - else - { - LOGW("HAL:Vendor accelerometer calibration file successfully loaded"); - inv_get_accel_bias(tmp_offset, NULL); - LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n", - tmp_offset[0], tmp_offset[1], tmp_offset[2]); - inv_set_accel_bias(accel_offset, mAccelAccuracy); - inv_get_accel_bias(tmp_offset, NULL); - LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n", - tmp_offset[0], tmp_offset[1], tmp_offset[2]); - } - } - /* End of Accel Calibration Load Method */ - - inv_set_device_properties(); - - /* disable driver master enable the first sensor goes on */ - masterEnable(0); - enableGyro(0); - enableAccel(0); - enableCompass(0); - - if (isLowPowerQuatEnabled()) { - enableLPQuaternion(0); - } - - onPower(0); - - if (isDmpDisplayOrientationOn()) { - enableDmpOrientation(!isDmpScreenAutoRotationEnabled()); - } - -} - -void MPLSensor::enable_iio_sysfs() -{ - VFUNC_LOG; - - char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN]; - FILE *tempFp = NULL; - - /* ignore failures */ - write_sysfs_int(mpu.in_timestamp_en, 1); - - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", - mpu.trigger_name, getTimestamp()); - tempFp = fopen(mpu.trigger_name, "r"); - if (tempFp == NULL) { - LOGE("HAL:could not open trigger name"); - } else { - if (fscanf(tempFp, "%s", iio_trigger_name) < 0) { - LOGE("HAL:could not read trigger name"); - } - fclose(tempFp); - } - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)", - iio_trigger_name, mpu.current_trigger, getTimestamp()); - tempFp = fopen(mpu.current_trigger, "w"); - if (tempFp == NULL) { - LOGE("HAL:could not open current trigger"); - } else { - if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) { - LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno); - } - } - - write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH); - - if (inv_get_iio_device_node(iio_device_node) < 0) { - LOGE("HAL:could retrive the iio device node"); - } - iio_fd = open(iio_device_node, O_RDONLY); - if (iio_fd < 0) { - LOGE("HAL:could not open iio device node"); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd); - } -} - -int MPLSensor::inv_constructor_init() -{ - VFUNC_LOG; - - inv_error_t result = inv_init_mpl(); - if (result) { - LOGE("HAL:inv_init_mpl() failed"); - return result; - } - result = inv_constructor_default_enable(); - result = inv_start_mpl(); - if (result) { - LOGE("HAL:inv_start_mpl() failed"); - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -int MPLSensor::inv_constructor_default_enable() -{ - VFUNC_LOG; - - inv_error_t result; - - result = inv_enable_quaternion(); - if (result) { - LOGE("HAL:Cannot enable quaternion\n"); - return result; - } - - result = inv_enable_in_use_auto_calibration(); - if (result) { - return result; - } - - // result = inv_enable_motion_no_motion(); - result = inv_enable_fast_nomot(); - if (result) { - return result; - } - - result = inv_enable_gyro_tc(); - if (result) { - return result; - } - - result = inv_enable_hal_outputs(); - if (result) { - return result; - } - - if (!mCompassSensor->providesCalibration()) { - /* Invensense compass calibration */ - LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled"); - result = inv_enable_vector_compass_cal(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else { - mFeatureActiveMask |= INV_COMPASS_CAL; - } - - // specify MPL's trust weight, used by compass algorithms - inv_vector_compass_cal_sensitivity(3); - - result = inv_enable_compass_bias_w_gyro(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_enable_heading_from_gyro(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_enable_magnetic_disturbance(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - result = inv_enable_9x_sensor_fusion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else { - // 9x sensor fusion enables Compass fit - mFeatureActiveMask |= INV_COMPASS_FIT; - } - - result = inv_enable_no_gyro_fusion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_enable_quat_accuracy_monitor(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/* TODO: create function pointers to calculate scale */ -void MPLSensor::inv_set_device_properties() -{ - VFUNC_LOG; - - unsigned short orient; - - inv_get_sensors_orientation(); - - inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE); - inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE); - - /* gyro setup */ - orient = inv_orientation_matrix_to_scalar(mGyroOrientation); - inv_set_gyro_orientation_and_scale(orient, 2000L << 15); - - /* accel setup */ - orient = inv_orientation_matrix_to_scalar(mAccelOrientation); - /* use for third party accel input subsystem driver - inv_set_accel_orientation_and_scale(orient, 1LL << 22); - */ - inv_set_accel_orientation_and_scale(orient, mAccelScale << 15); - - /* compass setup */ - signed char orientMtx[9]; - mCompassSensor->getOrientationMatrix(orientMtx); - orient = - inv_orientation_matrix_to_scalar(orientMtx); - long sensitivity; - sensitivity = mCompassSensor->getSensitivity(); - inv_set_compass_orientation_and_scale(orient, sensitivity); -} - -void MPLSensor::loadDMP() -{ - int res, fd; - FILE *fptr; - - if (isMpu3050()) { - //DMP support only for MPU6xxx/9xxx currently - return; - } - - /* load DMP firmware */ - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp()); - fd = open(mpu.firmware_loaded, O_RDONLY); - if(fd < 0) { - LOGE("HAL:could not open dmp state"); - return; - } - if(inv_read_dmp_state(fd)) { - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded"); - return; - } - - LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware); - fptr = fopen(mpu.dmp_firmware, "w"); - if(!fptr) { - LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno)); - return; - } - res = inv_load_dmp(fptr); - if(res < 0) { - LOGE("HAL:load DMP failed"); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); - } - fclose(fptr); -} - -void MPLSensor::inv_get_sensors_orientation() -{ - FILE *fptr; - - // get gyro orientation - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp()); - fptr = fopen(mpu.gyro_orient, "r"); - if (fptr != NULL) { - int om[9]; - fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", - &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], - &om[6], &om[7], &om[8]); - fclose(fptr); - - LOGV_IF(EXTRA_VERBOSE, - "HAL:gyro mounting matrix: " - "%+d %+d %+d %+d %+d %+d %+d %+d %+d", - om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); - - mGyroOrientation[0] = om[0]; - mGyroOrientation[1] = om[1]; - mGyroOrientation[2] = om[2]; - mGyroOrientation[3] = om[3]; - mGyroOrientation[4] = om[4]; - mGyroOrientation[5] = om[5]; - mGyroOrientation[6] = om[6]; - mGyroOrientation[7] = om[7]; - mGyroOrientation[8] = om[8]; - } else { - LOGE("HAL:Couldn't read gyro mounting matrix"); - } - - // get accel orientation - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp()); - fptr = fopen(mpu.accel_orient, "r"); - if (fptr != NULL) { - int om[9]; - fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", - &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], - &om[6], &om[7], &om[8]); - fclose(fptr); - - LOGV_IF(EXTRA_VERBOSE, - "HAL:accel mounting matrix: " - "%+d %+d %+d %+d %+d %+d %+d %+d %+d", - om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); - - mAccelOrientation[0] = om[0]; - mAccelOrientation[1] = om[1]; - mAccelOrientation[2] = om[2]; - mAccelOrientation[3] = om[3]; - mAccelOrientation[4] = om[4]; - mAccelOrientation[5] = om[5]; - mAccelOrientation[6] = om[6]; - mAccelOrientation[7] = om[7]; - mAccelOrientation[8] = om[8]; - } else { - LOGE("HAL:Couldn't read accel mounting matrix"); - } -} - -MPLSensor::~MPLSensor() -{ - VFUNC_LOG; - - mCompassSensor = NULL; - - /* Close open fds */ - if (iio_fd > 0) - close(iio_fd); - if( accel_fd > 0 ) - close(accel_fd ); - if (gyro_temperature_fd > 0) - close(gyro_temperature_fd); - if (sysfs_names_ptr) - free(sysfs_names_ptr); - - if (isDmpDisplayOrientationOn()) { - closeDmpOrientFd(); - } - - /* Turn off Gyro master enable */ - /* A workaround until driver handles it */ - /* TODO: Turn off and close all sensors */ - if(write_sysfs_int(mpu.chip_enable, 0) < 0) { - LOGE("HAL:could not disable gyro master enable"); - } - -#ifdef INV_PLAYBACK_DBG - inv_turn_off_data_logging(); - fclose(logfile); -#endif -} - -#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors) -#define A_ENABLED ((1 << ID_A) & enabled_sensors) -#define M_ENABLED ((1 << ID_M) & enabled_sensors) -#define O_ENABLED ((1 << ID_O) & enabled_sensors) -#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) -#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) -#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) - -/* TODO: this step is optional, remove? */ -int MPLSensor::setGyroInitialState() -{ - VFUNC_LOG; - - int res = 0; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp()); - int fd = open(mpu.gyro_fifo_rate, O_RDWR); - res = errno; - if(fd < 0) { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.gyro_fifo_rate, strerror(res), res); - return res; - } - res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ); - if(res < 0) { - LOGE("HAL:write_attribute_sensor : error writing %s with %d", - mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ); - return res; - } - - // Setting LPF is deprecated - - return 0; -} - -/* this applies to BMA250 Input Subsystem Driver only */ -int MPLSensor::setAccelInitialState() -{ - VFUNC_LOG; - - struct input_absinfo absinfo_x; - struct input_absinfo absinfo_y; - struct input_absinfo absinfo_z; - float value; - if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) && - !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) && - !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) { - value = absinfo_x.value; - mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X; - value = absinfo_y.value; - mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y; - value = absinfo_z.value; - mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z; - } - return 0; -} - -int MPLSensor::onPower(int en) -{ - VFUNC_LOG; - - int res; - - int curr_power_state; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - en, mpu.power_state, getTimestamp()); - res = read_sysfs_int(mpu.power_state, &curr_power_state); - if (res < 0) { - LOGE("HAL:Error reading power state"); - // will set power_state anyway - curr_power_state = -1; - } - if (en != curr_power_state) { - if((res = write_sysfs_int(mpu.power_state, en)) < 0) { - LOGE("HAL:Couldn't write power state"); - } - } else { - LOGV_IF(EXTRA_VERBOSE, - "HAL:Power state already enable/disable curr=%d new=%d", - curr_power_state, en); - } - return res; -} - -int MPLSensor::onDMP(int en) -{ - VFUNC_LOG; - - int res = -1; - int status; - - //Sequence to enable DMP - //1. Turn On power if not already on - //2. Load DMP image if not already loaded - //3. Either Gyro or Accel must be enabled/configured before next step - //4. Enable DMP - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", - mpu.firmware_loaded, getTimestamp()); - res = read_sysfs_int(mpu.firmware_loaded, &status); - if (res < 0){ - LOGE("HAL:ERR can't get firmware_loaded status"); - return res; - } - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status); - - if (status) { - //Write only if curr DMP state <> request - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", - mpu.dmp_on, getTimestamp()); - if (read_sysfs_int(mpu.dmp_on, &status) < 0) { - LOGE("HAL:ERR can't read DMP state"); - } else if (status != en) { - res = write_sysfs_int(mpu.dmp_on, en); - //Enable DMP interrupt - if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { - LOGE("HAL:ERR can't en/dis DMP interrupt"); - } - } else { - res = 0; //DMP already set as requested - } - } else { - LOGE("HAL:ERR No DMP image"); - } - return res; -} - -int MPLSensor::checkLPQuaternion(void) -{ - VFUNC_LOG; - - return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); -} - -int MPLSensor::enableLPQuaternion(int en) -{ - VFUNC_LOG; - - if (!en) { - enableQuaternionData(0); - onDMP(0); - mFeatureActiveMask &= ~INV_DMP_QUATERNION; - LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled"); - } else { - if (enableQuaternionData(1) < 0 || onDMP(1) < 0) { - LOGE("HAL:ERR can't enable LP Quaternion"); - } else { - mFeatureActiveMask |= INV_DMP_QUATERNION; - LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled"); - } - } - return 0; -} - -int MPLSensor::enableQuaternionData(int en) -{ - int res = 0; - VFUNC_LOG; - - // Enable DMP quaternion - res = write_sysfs_int(mpu.quaternion_on, en); - - if (!en) { - LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems"); - } else { - - LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems"); - } - write_sysfs_int(mpu.in_quat_r_en, en); - write_sysfs_int(mpu.in_quat_x_en, en); - write_sysfs_int(mpu.in_quat_y_en, en); - write_sysfs_int(mpu.in_quat_z_en, en); - - LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off"); - - if (!en) { - inv_quaternion_sensor_was_turned_off(); - } - - return res; -} - -int MPLSensor::enableTap(int /*en*/) -{ - VFUNC_LOG; - - return 0; -} - -int MPLSensor::enableFlick(int /*en*/) -{ - VFUNC_LOG; - - return 0; -} - -int MPLSensor::enablePedometer(int /*en*/) -{ - VFUNC_LOG; - - return 0; -} - -int MPLSensor::masterEnable(int en) -{ - VFUNC_LOG; - return write_sysfs_int(mpu.chip_enable, en); -} - -int MPLSensor::enableGyro(int en) -{ - VFUNC_LOG; - - /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ - int res; - - /* need to also turn on/off the master enable */ - res = write_sysfs_int(mpu.gyro_enable, en); - - if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); - inv_gyro_was_turned_off(); - } else { - write_sysfs_int(mpu.gyro_x_fifo_enable, en); - write_sysfs_int(mpu.gyro_y_fifo_enable, en); - res = write_sysfs_int(mpu.gyro_z_fifo_enable, en); - } - - return res; -} - -int MPLSensor::enableAccel(int en) -{ - VFUNC_LOG; - - /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ - int res; - - /* need to also turn on/off the master enable */ - res = write_sysfs_int(mpu.accel_enable, en); - - if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); - inv_accel_was_turned_off(); - } else { - write_sysfs_int(mpu.accel_x_fifo_enable, en); - write_sysfs_int(mpu.accel_y_fifo_enable, en); - res = write_sysfs_int(mpu.accel_z_fifo_enable, en); - } - - return res; -} - -int MPLSensor::enableCompass(int en) -{ - VFUNC_LOG; - - int res = mCompassSensor->enable(ID_M, en); - if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off"); - inv_compass_was_turned_off(); - } - return res; -} - -void MPLSensor::computeLocalSensorMask(int enabled_sensors) -{ - VFUNC_LOG; - - do { - if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { - LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); - mLocalSensorMask = ALL_MPL_SENSORS_NP; - break; - } - - if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) { - /* Invensense compass cal */ - LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); - mLocalSensorMask = 0; - break; - } - - if (GY_ENABLED) { - LOGV_IF(ENG_VERBOSE, "G ENABLED"); - mLocalSensorMask |= INV_THREE_AXIS_GYRO; - } else { - LOGV_IF(ENG_VERBOSE, "G DISABLED"); - mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; - } - - if (A_ENABLED) { - LOGV_IF(ENG_VERBOSE, "A ENABLED"); - mLocalSensorMask |= INV_THREE_AXIS_ACCEL; - } else { - LOGV_IF(ENG_VERBOSE, "A DISABLED"); - mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; - } - - /* Invensense compass calibration */ - if (M_ENABLED) { - LOGV_IF(ENG_VERBOSE, "M ENABLED"); - mLocalSensorMask |= INV_THREE_AXIS_COMPASS; - } else { - LOGV_IF(ENG_VERBOSE, "M DISABLED"); - mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; - } - } while (0); -} - -int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) { - LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name); - return (this->*enabler)(en); -} - -int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) { - VFUNC_LOG; - - inv_error_t res = -1; - bool store_cal = false; - bool ext_compass_changed = false; - - // Sequence to enable or disable a sensor - // 1. enable Power state - // 2. reset master enable (=0) - // 3. enable or disable a sensor - // 4. set master enable (=1) - - pthread_mutex_lock(&GlobalHalMutex); - - uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) - | (1 << MagneticField); - uint32_t all_integrated_changeables = all_changeables; - - if (!mCompassSensor->isIntegrated()) { - ext_compass_changed = changed & (1 << MagneticField); - all_integrated_changeables = all_changeables & ~(1 << MagneticField); - } - - if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) { - /* ensure power state is on */ - onPower(1); - - /* reset master enable */ - res = masterEnable(0); - if(res < 0) { - goto unlock_res; - } - } - - LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors); - - if (changed & ((1 << Gyro) | (1 << RawGyro))) { - res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro); - if(res < 0) { - goto unlock_res; - } - } - - if (changed & (1 << Accelerometer)) { - res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel); - if(res < 0) { - goto unlock_res; - } - } - - if (changed & (1 << MagneticField)) { - /* Invensense compass calibration */ - res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass); - if(res < 0) { - goto unlock_res; - } - } - - if ( isLowPowerQuatEnabled() ) { - // Enable LP Quat - if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity)))) { - if (!(changed & all_integrated_changeables)) { - /* ensure power state is on */ - onPower(1); - /* reset master enable */ - res = masterEnable(0); - if(res < 0) { - goto unlock_res; - } - } - if (!checkLPQuaternion()) { - enableLPQuaternion(1); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled"); - } - } else if (checkLPQuaternion()) { - enableLPQuaternion(0); - } - } - - if (changed & all_integrated_changeables) { - if (sensors & - (INV_THREE_AXIS_GYRO - | INV_THREE_AXIS_ACCEL - | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { - if ( isLowPowerQuatEnabled() || - (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) { - // disable DMP event interrupt only (w/ data interrupt) - if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { - res = -1; - LOGE("HAL:ERR can't disable DMP event interrupt"); - goto unlock_res; - } - } - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - // enable DMP - onDMP(1); - - res = enableAccel(1); - if(res < 0) { - goto unlock_res; - } - - if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { - res = turnOffAccelFifo(); - } - if(res < 0) { - goto unlock_res; - } - } - - res = masterEnable(1); - if(res < 0) { - goto unlock_res; - } - } else { // all sensors idle -> reduce power - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - // enable DMP - onDMP(1); - // enable DMP event interrupt only (no data interrupt) - if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) { - res = -1; - LOGE("HAL:ERR can't enable DMP event interrupt"); - } - res = enableAccel(1); - if(res < 0) { - goto unlock_res; - } - if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { - res = turnOffAccelFifo(); - } - if(res < 0) { - goto unlock_res; - } - res = masterEnable(1); - if(res < 0) { - goto unlock_res; - } - } - else { - res = onPower(0); - if(res < 0) { - goto unlock_res; - } - } - store_cal = true; - } - } else if (ext_compass_changed && - !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL - | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) { - store_cal = true; - } - - if (store_cal || ((changed & all_changeables) != all_changeables)) { - storeCalibration(); - } - -unlock_res: - pthread_mutex_unlock(&GlobalHalMutex); - return res; -} - -/* Store calibration file */ -void MPLSensor::storeCalibration() -{ - if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) { - int res = inv_store_calibration(); - if (res) { - LOGE("HAL:Cannot store calibration on file"); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated"); - } - } -} - -void MPLSensor::cbProcData() -{ - mNewData = 1; - mSampleCount++; - LOGV_IF(EXTRA_VERBOSE, "HAL:new data"); -} - -/* these handlers transform mpl data into one of the Android sensor types */ -int MPLSensor::gyroHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d", - s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); - return update; -} - -int MPLSensor::rawGyroHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d", - s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); - return update; -} - -int MPLSensor::accelHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_accelerometer( - s->acceleration.v, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d", - s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], - s->timestamp, update); - mAccelAccuracy = status; - return update; -} - -int MPLSensor::compassHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int update; - update = inv_get_sensor_type_magnetic_field( - s->magnetic.v, &s->magnetic.status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d", - s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update); - mCompassAccuracy = s->magnetic.status; - return update; -} - -int MPLSensor::rvHandler(sensors_event_t* s) -{ - // rotation vector does not have an accuracy or status - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d", - s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update); - return update; -} - -int MPLSensor::laHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_linear_acceleration( - s->gyro.v, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d", - s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); - return update; -} - -int MPLSensor::gravHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d", - s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); - return update; -} - -int MPLSensor::orienHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int update; - update = inv_get_sensor_type_orientation( - s->orientation.v, &s->orientation.status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d", - s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update); - return update; -} - -int MPLSensor::enable(int32_t handle, int en) -{ - VFUNC_LOG; - - android::String8 sname; - int what = -1, err = 0; - - switch (handle) { - case ID_SO: - sname = "Screen Orientation"; - LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle, - (mDmpOrientationEnabled? "en": "dis"), - (en? "en" : "dis")); - enableDmpOrientation(en && isDmpDisplayOrientationOn()); - /* TODO: stop manually testing/using 0 and 1 instead of - * false and true, but just use 0 and non-0. - * This allows passing 0 and non-0 ints around instead of - * having to convert to 1 and test against 1. - */ - mDmpOrientationEnabled = en; - return 0; - case ID_A: - what = Accelerometer; - sname = "Accelerometer"; - break; - case ID_M: - what = MagneticField; - sname = "MagneticField"; - break; - case ID_O: - what = Orientation; - sname = "Orientation"; - break; - case ID_GY: - what = Gyro; - sname = "Gyro"; - break; - case ID_RG: - what = RawGyro; - sname = "RawGyro"; - break; - case ID_GR: - what = Gravity; - sname = "Gravity"; - break; - case ID_RV: - what = RotationVector; - sname = "RotationVector"; - break; - case ID_LA: - what = LinearAccel; - sname = "LinearAccel"; - break; - default: //this takes care of all the gestures - what = handle; - sname = "Others"; - break; - } - - if (uint32_t(what) >= numSensors) - return -EINVAL; - - int newState = en ? 1 : 0; - unsigned long sen_mask; - - LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle, - ((mEnabled & (1 << what)) ? "en" : "dis"), - ((uint32_t(newState) << what) ? "en" : "dis")); - LOGV_IF(PROCESS_VERBOSE, - "HAL:%s sensor state change what=%d", sname.string(), what); - - if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { - short flags = newState; - uint32_t lastEnabled = mEnabled, changed = 0; - - mEnabled &= ~(1 << what); - mEnabled |= (uint32_t(flags) << what); - - LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle); - LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags); - computeLocalSensorMask(mEnabled); - LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled); - sen_mask = mLocalSensorMask & mMasterSensorMask; - mSensorMask = sen_mask; - LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask); - - switch (what) { - case Gyro: - case RawGyro: - case Accelerometer: - case MagneticField: - if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity))) && - ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { - changed |= (1 << what); - } - break; - - case Orientation: - case RotationVector: - case LinearAccel: - case Gravity: - if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity)))) || - (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity))))) { - for (int i = Gyro; i <= MagneticField; i++) { - if (!(mEnabled & (1 << i))) { - changed |= (1 << i); - } - } - } - break; - } - LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed); - enableSensors(sen_mask, flags, changed); - } - -#ifdef INV_PLAYBACK_DBG - /* apparently the logging needs to be go through this sequence - to properly flush the log file */ - inv_turn_off_data_logging(); - fclose(logfile); - logfile = fopen("/data/playback.bin", "ab"); - if (logfile) - inv_turn_on_data_logging(logfile); -#endif - - return err; -} - -int MPLSensor::setDelay(int32_t handle, int64_t ns) -{ - VFUNC_LOG; - - android::String8 sname; - int what = -1; - - switch (handle) { - case ID_SO: - return update_delay(); - case ID_A: - what = Accelerometer; - sname = "Accelerometer"; - break; - case ID_M: - what = MagneticField; - sname = "MagneticField"; - break; - case ID_O: - what = Orientation; - sname = "Orientation"; - break; - case ID_GY: - what = Gyro; - sname = "Gyro"; - break; - case ID_RG: - what = RawGyro; - sname = "RawGyro"; - break; - case ID_GR: - what = Gravity; - sname = "Gravity"; - break; - case ID_RV: - what = RotationVector; - sname = "RotationVector"; - break; - case ID_LA: - what = LinearAccel; - sname = "LinearAccel"; - break; - default: // this takes care of all the gestures - what = handle; - sname = "Others"; - break; - } - -#if 0 - // skip the 1st call for enalbing sensors called by ICS/JB sensor service - static int counter_delay = 0; - if (!(mEnabled & (1 << what))) { - counter_delay = 0; - } else { - if (++counter_delay == 1) { - return 0; - } - else { - counter_delay = 0; - } - } -#endif - - if (uint32_t(what) >= numSensors) - return -EINVAL; - - if (ns < 0) - return -EINVAL; - - LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns); - - // limit all rates to reasonable ones */ - if (ns < 5000000LL) { - ns = 5000000LL; - } - - /* store request rate to mDelays array for each sensor */ - mDelays[what] = ns; - - switch (what) { - case Gyro: - case RawGyro: - case Accelerometer: - for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); - i++) { - if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { - LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); - return 0; - } - } - break; - - case MagneticField: - if (mCompassSensor->isIntegrated() && - (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || - ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) || - ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) { - LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel"); - return 0; - } - break; - - case Orientation: - case RotationVector: - case LinearAccel: - case Gravity: - if (isLowPowerQuatEnabled()) { - LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ"); - break; - } - - for (int i = 0; i < numSensors; i++) { - if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { - LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); - return 0; - } - } - break; - } - - int res = update_delay(); - return res; -} - -int MPLSensor::update_delay() { - VHANDLER_LOG; - - int res = 0; - int64_t got; - - pthread_mutex_lock(&GlobalHalMutex); - if (mEnabled) { - int64_t wanted = 1000000000; - int64_t wanted_3rd_party_sensor = 1000000000; - - // Sequence to change sensor's FIFO rate - // 1. enable Power state - // 2. reset master enable - // 3. Update delay - // 4. set master enable - - // ensure power on - onPower(1); - - // reset master enable - masterEnable(0); - - /* search the minimum delay requested across all enabled sensors */ - for (int i = 0; i < numSensors; i++) { - if (mEnabled & (1 << i)) { - int64_t ns = mDelays[i]; - wanted = wanted < ns ? wanted : ns; - } - } - - // same delay for 3rd party Accel or Compass - wanted_3rd_party_sensor = wanted; - - /* mpl rate in us in future maybe different for - gyro vs compass vs accel */ - int rateInus = (int)wanted / 1000LL; - int mplGyroRate = rateInus; - int mplAccelRate = rateInus; - int mplCompassRate = rateInus; - - LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : " - "%llu ns, mpl rate: %d us, (%.2f Hz)", - wanted, rateInus, 1000000000.f / wanted); - - /* set rate in MPL */ - /* compass can only do 100Hz max */ - inv_set_gyro_sample_rate(mplGyroRate); - inv_set_accel_sample_rate(mplAccelRate); - inv_set_compass_sample_rate(mplCompassRate); -#ifdef LIBMLLITE_FROM_SOURCE - inv_set_linear_acceleration_sample_rate(rateInus); - inv_set_gravity_sample_rate(rateInus); -#endif - - /* TODO: Test 200Hz */ - // inv_set_gyro_sample_rate(5000); - LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate); - LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate); - LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate); - - int enabled_sensors = mEnabled; - int tempFd = -1; - if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { - if (isLowPowerQuatEnabled() || - (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) { - bool setDMPrate= 0; - // Set LP Quaternion sample rate if enabled - if (checkLPQuaternion()) { - if (wanted < RATE_200HZ) { - enableLPQuaternion(0); - } else { - inv_set_quat_sample_rate(rateInus); - setDMPrate= 1; - } - } - - if (checkDMPOrientation() || setDMPrate==1) { - getDmpRate(&wanted); - } - } - - int64_t tempRate = wanted; - LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); - //nsToHz - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", - 1000000000.f / tempRate, mpu.gyro_fifo_rate, - getTimestamp()); - tempFd = open(mpu.gyro_fifo_rate, O_RDWR); - res = write_attribute_sensor(tempFd, 1000000000.f / tempRate); - if(res < 0) { - LOGE("HAL:GYRO update delay error"); - } - - //nsToHz (BMA250) - if(USE_THIRD_PARTY_ACCEL) { - LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", - wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate, - getTimestamp()); - tempFd = open(mpu.accel_fifo_rate, O_RDWR); - res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L); - LOGE_IF(res < 0, "HAL:ACCEL update delay error"); - } - - if (!mCompassSensor->isIntegrated()) { - LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor); - mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor); - got = mCompassSensor->getDelay(ID_M); - inv_set_compass_sample_rate(got / 1000); - } - - } else { - - if (GY_ENABLED) { - wanted = (mDelays[Gyro] <= mDelays[RawGyro]? - (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): - (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - getDmpRate(&wanted); - } - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", - 1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp()); - tempFd = open(mpu.gyro_fifo_rate, O_RDWR); - res = write_attribute_sensor(tempFd, 1000000000.f / wanted); - LOGE_IF(res < 0, "HAL:GYRO update delay error"); - } - - if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */ - if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { - wanted = mDelays[Gyro]; - } - else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) { - wanted = mDelays[RawGyro]; - - } else { - wanted = mDelays[Accelerometer]; - } - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - getDmpRate(&wanted); - } - - /* TODO: use function pointers to calculate delay value specific to vendor */ - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", - 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp()); - tempFd = open(mpu.accel_fifo_rate, O_RDWR); - if(USE_THIRD_PARTY_ACCEL) { - //BMA250 in ms - res = write_attribute_sensor(tempFd, wanted / 1000000L); - } - else { - //MPUxxxx in hz - res = write_attribute_sensor(tempFd, 1000000000.f/wanted); - } - LOGE_IF(res < 0, "HAL:ACCEL update delay error"); - } - - /* Invensense compass calibration */ - if (M_ENABLED) { - if (!mCompassSensor->isIntegrated()) { - wanted = mDelays[MagneticField]; - } else { - if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) { - wanted = mDelays[Gyro]; - } - else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) { - wanted = mDelays[RawGyro]; - } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) { - wanted = mDelays[Accelerometer]; - } else { - wanted = mDelays[MagneticField]; - } - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - getDmpRate(&wanted); - } - } - - mCompassSensor->setDelay(ID_M, wanted); - got = mCompassSensor->getDelay(ID_M); - inv_set_compass_sample_rate(got / 1000); - } - - } - - unsigned long sensors = mLocalSensorMask & mMasterSensorMask; - if (sensors & - (INV_THREE_AXIS_GYRO - | INV_THREE_AXIS_ACCEL - | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { - res = masterEnable(1); - } else { // all sensors idle -> reduce power - res = onPower(0); - } - } - pthread_mutex_unlock(&GlobalHalMutex); - return res; -} - -/* For Third Party Accel Input Subsystem Drivers only */ -/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */ -int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count) -{ - VHANDLER_LOG; - - if (count < 1) - return -EINVAL; - - ssize_t n = mAccelInputReader.fill(accel_fd); - if (n < 0) { - LOGE("HAL:missed accel events, exit"); - return n; - } - - int numEventReceived = 0; - input_event const* event; - int done = 0; - - while (!done && count && mAccelInputReader.readEvent(&event)) { - int type = event->type; - if (type == EV_ABS) { - if (event->code == EVENT_TYPE_ACCEL_X) { - mPendingMask |= 1 << Accelerometer; - mCachedAccelData[0] = event->value; - } else if (event->code == EVENT_TYPE_ACCEL_Y) { - mPendingMask |= 1 << Accelerometer; - mCachedAccelData[1] = event->value; - } else if (event->code == EVENT_TYPE_ACCEL_Z) { - mPendingMask |= 1 << Accelerometer; - mCachedAccelData[2] =event-> value; - } - } else if (type == EV_SYN) { - done = 1; - if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { - inv_build_accel(mCachedAccelData, 0, getTimestamp()); - } - } else { - LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mAccelInputReader.next(); - } - - return numEventReceived; -} - -/** - * Should be called after reading at least one of gyro - * compass or accel data. (Also okay for handling all of them). - * @returns 0, if successful, error number if not. - */ -/* TODO: This should probably be called "int readEvents(...)" - * and readEvents() called "void cacheData(void)". - */ -int MPLSensor::executeOnData(sensors_event_t* data, int count) -{ - VFUNC_LOG; - - inv_execute_on_data(); - - int numEventReceived = 0; - - long msg; - msg = inv_get_message_level_0(1); - if (msg) { - if (msg & INV_MSG_MOTION_EVENT) { - LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n"); - } - if (msg & INV_MSG_NO_MOTION_EVENT) { - LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n"); - /* after the first no motion, the gyro should be - calibrated well */ - mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH; - /* if gyros are on and we got a no motion, set a flag - indicating that the cal file can be written. */ - mHaveGoodMpuCal = true; - } - } - - // load up virtual sensors - for (int i = 0; i < numSensors; i++) { - int update; - if (mEnabled & (1 << i)) { - update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i); - mPendingMask |= (1 << i); - - if (update && (count > 0)) { - *data++ = mPendingEvents[i]; - count--; - numEventReceived++; - } - } - } - - return numEventReceived; -} - -// collect data for MPL (but NOT sensor service currently), from driver layer -/* TODO: FIX! data and count are not used, results is hardcoded to 0 */ -/* TODO: This should probably be called "void cacheEvents(void)" - * And executeOnData() should be int readEvents(data,count) - */ -int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) { - - - int lp_quaternion_on = 0, nbyte; - int i, mask = 0, numEventReceived = 0, - sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) + - (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0); - char *rdata = mIIOBuffer; - - nbyte= (8 * sensors + 8) * 1; - - if (isLowPowerQuatEnabled()) { - lp_quaternion_on = checkLPQuaternion(); - if (lp_quaternion_on) { - nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data - } - } - - ssize_t rsize = read(iio_fd, rdata, nbyte); - if (sensors == 0) { - rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); - } - -#ifdef TESTING - LOGI("get one sample of IIO data with size: %d", rsize); - LOGI("sensors: %d", sensors); - - LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d", - *((short *) (rdata + 0)), *((short *) (rdata + 2)), - *((short *) (rdata + 4))); - LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d", - *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), - *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), - *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); - - LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS & - mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d", - *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), - *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), - *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))); -#endif - - if (rsize != nbyte) { - LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)", - rsize, nbyte, sensors, errno, strerror(errno)); - rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); - return -1; - } - - if (isLowPowerQuatEnabled() && lp_quaternion_on) { - - for (i=0; i< 4; i++) { - mCachedQuaternionData[i]= *(long*)rdata; - rdata += sizeof(long); - } - } - - for (i = 0; i < 3; i++) { - if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { - mCachedGyroData[i] = *((short *) (rdata + i * 2)); - } - if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { - mCachedAccelData[i] = *((short *) (rdata + i * 2 + - ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); - } - if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) { - mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1))); - } - } - - mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0)); - if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() && - (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) { - mask |= 1 << MagneticField; - } - - mSensorTimestamp = *((long long *) (rdata + 8 * sensors)); - if (mCompassSensor->isIntegrated()) { - mCompassTimestamp = mSensorTimestamp; - } - - if (mask & (1 << Gyro)) { - // send down temperature every 0.5 seconds - // with timestamp measured in "driver" layer - if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) { - mTempCurrentTime = mSensorTimestamp; - long long temperature[2]; - if(inv_read_temperature(temperature) == 0) { - LOGV_IF(INPUT_DATA, - "HAL:inv_read_temperature = %lld, timestamp= %lld", - temperature[0], temperature[1]); - inv_build_temp(temperature[0], temperature[1]); - } -#ifdef TESTING - long bias[3], temp, temp_slope[3]; - inv_get_gyro_bias(bias, &temp); - inv_get_gyro_ts(temp_slope); - - LOGI("T: %.3f " - "GB: %+13f %+13f %+13f " - "TS: %+13f %+13f %+13f " - "\n", - (float)temperature[0] / 65536.f, - (float)bias[0] / 65536.f / 16.384f, - (float)bias[1] / 65536.f / 16.384f, - (float)bias[2] / 65536.f / 16.384f, - temp_slope[0] / 65536.f, - temp_slope[1] / 65536.f, - temp_slope[2] / 65536.f); -#endif - } - - mPendingMask |= 1 << Gyro; - mPendingMask |= 1 << RawGyro; - - if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { - inv_build_gyro(mCachedGyroData, mSensorTimestamp); - LOGV_IF(INPUT_DATA, - "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld", - mCachedGyroData[0], mCachedGyroData[1], - mCachedGyroData[2], mSensorTimestamp); - } - } - - if (mask & (1 << Accelerometer)) { - mPendingMask |= 1 << Accelerometer; - if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { - inv_build_accel(mCachedAccelData, 0, mSensorTimestamp); - LOGV_IF(INPUT_DATA, - "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld", - mCachedAccelData[0], mCachedAccelData[1], - mCachedAccelData[2], mSensorTimestamp); - } - } - - if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) { - int status = 0; - if (mCompassSensor->providesCalibration()) { - status = mCompassSensor->getAccuracy(); - status |= INV_CALIBRATED; - } - if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { - inv_build_compass(mCachedCompassData, status, - mCompassTimestamp); - LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", - mCachedCompassData[0], mCachedCompassData[1], - mCachedCompassData[2], mCompassTimestamp); - } - } - - if (isLowPowerQuatEnabled() && lp_quaternion_on) { - - inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp); - LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld", - mCachedQuaternionData[0], mCachedQuaternionData[1], - mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp); - } - - return numEventReceived; -} - -/* use for both MPUxxxx and third party compass */ -int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count) -{ - VHANDLER_LOG; - - if (count < 1) - return -EINVAL; - - int numEventReceived = 0; - int done = 0; - - done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); -#ifdef COMPASS_YAS53x - if (mCompassSensor->checkCoilsReset()) { - //Reset relevant compass settings - resetCompass(); - } -#endif - if (done > 0) { - int status = 0; - if (mCompassSensor->providesCalibration()) { - status = mCompassSensor->getAccuracy(); - status |= INV_CALIBRATED; - } - if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { - inv_build_compass(mCachedCompassData, status, - mCompassTimestamp); - LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", - mCachedCompassData[0], mCachedCompassData[1], - mCachedCompassData[2], mCompassTimestamp); - } - } - - return numEventReceived; -} - -#ifdef COMPASS_YAS53x -int MPLSensor::resetCompass() -{ - VFUNC_LOG; - - //Reset compass cal if enabled - if (mFeatureActiveMask & INV_COMPASS_CAL) { - LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal"); - inv_init_vector_compass_cal(); - } - - //Reset compass fit if enabled - if (mFeatureActiveMask & INV_COMPASS_FIT) { - LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit"); - inv_init_compass_fit(); - } - - return 0; -} -#endif - -int MPLSensor::getFd() const -{ - VFUNC_LOG; - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd); - return iio_fd; -} - -int MPLSensor::getAccelFd() const -{ - VFUNC_LOG; - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd); - return accel_fd; -} - -int MPLSensor::getCompassFd() const -{ - VFUNC_LOG; - int fd = mCompassSensor->getFd(); - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd); - return fd; -} - -int MPLSensor::turnOffAccelFifo() { - int i, res; - char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable, - mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable}; - - for (i = 0; i < 3; i++) { - res = write_sysfs_int(accel_fifo_enable[i], 0); - if (res < 0) { - return res; - } - } - return 0; -} - -int MPLSensor::enableDmpOrientation(int en) -{ - VFUNC_LOG; - /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ - int res = 0; - int enabled_sensors = mEnabled; - - if (isMpu3050()) - return res; - - pthread_mutex_lock(&GlobalHalMutex); - - // on power if not already On - res = onPower(1); - // reset master enable - res = masterEnable(0); - - if (en) { - //Enable DMP orientation - if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { - LOGE("HAL:ERR can't enable Android orientation"); - res = -1; // indicate an err - } - - // open DMP Orient Fd - res = openDmpOrientFd(); - - // enable DMP - res = onDMP(1); - - // default DMP output rate to FIFO - if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) { - LOGE("HAL:ERR can't default DMP output rate"); - } - - // set DMP rate to 200Hz - if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { - res = -1; - LOGE("HAL:ERR can't set DMP rate to 200Hz"); - } - - // enable accel engine - res = enableAccel(1); - - // disable accel FIFO - if (!A_ENABLED) { - res = turnOffAccelFifo(); - } - - mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION; - } else { - // disable DMP - res = onDMP(0); - - // disable accel engine - if (!A_ENABLED) { - res = enableAccel(0); - } - } - - res = masterEnable(1); - pthread_mutex_unlock(&GlobalHalMutex); - return res; -} - -int MPLSensor::openDmpOrientFd() -{ - VFUNC_LOG; - - if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) { - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened"); - return -1; - } - - dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK); - if (dmp_orient_fd < 0) { - LOGE("HAL:ERR couldn't open dmpOrient node"); - return -1; - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd); - return 0; - } -} - -int MPLSensor::closeDmpOrientFd() -{ - VFUNC_LOG; - if (dmp_orient_fd >= 0) - close(dmp_orient_fd); - return 0; -} - -int MPLSensor::dmpOrientHandler(int orient) -{ - VFUNC_LOG; - - LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient); - return 0; -} - -int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) { - VFUNC_LOG; - - char dummy[4]; - int screen_orientation = 0; - FILE *fp; - - fp = fopen(mpu.event_display_orientation, "r"); - if (fp == NULL) { - LOGE("HAL:cannot open event_display_orientation"); - return 0; - } - fscanf(fp, "%d\n", &screen_orientation); - fclose(fp); - - int numEventReceived = 0; - - if (mDmpOrientationEnabled && count > 0) { - sensors_event_t temp; - - bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */ - temp.version = sizeof(sensors_event_t); - temp.sensor = ID_SO; -#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION - temp.type = SENSOR_TYPE_SCREEN_ORIENTATION; - temp.screen_orientation = screen_orientation; -#endif - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; - - *data++ = temp; - count--; - numEventReceived++; - } - - // read dummy data per driver's request - dmpOrientHandler(screen_orientation); - read(dmp_orient_fd, dummy, 4); - - return numEventReceived; -} - -int MPLSensor::getDmpOrientFd() -{ - VFUNC_LOG; - - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd); - return dmp_orient_fd; - -} - -int MPLSensor::checkDMPOrientation() -{ - VFUNC_LOG; - return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0); -} - -int MPLSensor::getDmpRate(int64_t *wanted) -{ - if (checkDMPOrientation() || checkLPQuaternion()) { - // set DMP output rate to FIFO - write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted); - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted); - - //DMP running rate must be @ 200Hz - *wanted= RATE_200HZ; - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted); - } - return 0; -} - -int MPLSensor::getPollTime() -{ - VHANDLER_LOG; - return mPollTime; -} - -bool MPLSensor::hasPendingEvents() const -{ - VHANDLER_LOG; - // if we are using the polling workaround, force the main - // loop to check for data every time - return (mPollTime != -1); -} - -/* TODO: support resume suspend when we gain more info about them*/ -void MPLSensor::sleepEvent() -{ - VFUNC_LOG; -} - -void MPLSensor::wakeEvent() -{ - VFUNC_LOG; -} - -int MPLSensor::inv_float_to_q16(float *fdata, long *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (long)(fdata[0] * 65536.f); - ldata[1] = (long)(fdata[1] * 65536.f); - ldata[2] = (long)(fdata[2] * 65536.f); - return 0; -} - -int MPLSensor::inv_long_to_q16(long *fdata, long *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (fdata[1] * 65536.f); - ldata[1] = (fdata[2] * 65536.f); - ldata[2] = (fdata[3] * 65536.f); - return 0; -} - -int MPLSensor::inv_float_to_round(float *fdata, long *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (long)fdata[0]; - ldata[1] = (long)fdata[1]; - ldata[2] = (long)fdata[2]; - return 0; -} - -int MPLSensor::inv_float_to_round2(float *fdata, short *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (short)fdata[0]; - ldata[1] = (short)fdata[1]; - ldata[2] = (short)fdata[2]; - return 0; -} - -int MPLSensor::inv_read_temperature(long long *data) -{ - VHANDLER_LOG; - - int count = 0; - char raw_buf[40]; - long raw = 0; - - long long timestamp = 0; - - memset(raw_buf, 0, sizeof(raw_buf)); - count = read_attribute_sensor(gyro_temperature_fd, raw_buf, - sizeof(raw_buf)); - if(count < 1) { - LOGE("HAL:error reading gyro temperature"); - return -1; - } - - count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp); - - if(count < 0) { - return -1; - } - - LOGV_IF(ENG_VERBOSE, - "HAL:temperature raw = %ld, timestamp = %lld, count = %d", - raw, timestamp, count); - data[0] = raw; - data[1] = timestamp; - - return 0; -} - -int MPLSensor::inv_read_dmp_state(int fd) -{ - VFUNC_LOG; - - if(fd < 0) - return -1; - - int count = 0; - char raw_buf[10]; - short raw = 0; - - memset(raw_buf, 0, sizeof(raw_buf)); - count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); - if(count < 1) { - LOGE("HAL:error reading dmp state"); - close(fd); - return -1; - } - count = sscanf(raw_buf, "%hd", &raw); - if(count < 0) { - LOGE("HAL:dmp state data is invalid"); - close(fd); - return -1; - } - LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count); - close(fd); - return (int)raw; -} - -int MPLSensor::inv_read_sensor_bias(int fd, long *data) -{ - VFUNC_LOG; - - if(fd == -1) { - return -1; - } - - char buf[50]; - char x[15], y[15], z[15]; - - memset(buf, 0, sizeof(buf)); - int count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 1) { - LOGE("HAL:Error reading gyro bias"); - return -1; - } - count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z); - if(count) { - /* scale appropriately for MPL */ - LOGV_IF(ENG_VERBOSE, - "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)", - atol(x), atol(y), atol(z)); - - data[0] = (long)(atol(x) / 10000 * (1L << 16)); - data[1] = (long)(atol(y) / 10000 * (1L << 16)); - data[2] = (long)(atol(z) / 10000 * (1L << 16)); - - LOGV_IF(ENG_VERBOSE, - "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)", - data[0], data[1], data[2]); - } - return 0; -} - -/** fill in the sensor list based on which sensors are configured. - * return the number of configured sensors. - * parameter list must point to a memory region of at least 7*sizeof(sensor_t) - * parameter len gives the length of the buffer pointed to by list - */ -int MPLSensor::populateSensorList(struct sensor_t *list, int len) -{ - VFUNC_LOG; - - int numsensors; - - if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { - LOGE("HAL:sensor list too small, not populating."); - return -(sizeof(sSensorList) / sizeof(sensor_t)); - } - - /* fill in the base values */ - memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t))); - - /* first add gyro, accel and compass to the list */ - - /* fill in gyro/accel values */ - fillGyro(chip_ID, list); - fillAccel(chip_ID, list); - - // TODO: need fixes for unified HAL and 3rd-party solution - mCompassSensor->fillList(&list[MagneticField]); - - if(1) { - numsensors = (sizeof(sSensorList) / sizeof(sensor_t)); - /* all sensors will be added to the list - fill in orientation values */ - fillOrientation(list); - /* fill in rotation vector values */ - fillRV(list); - /* fill in gravity values */ - fillGravity(list); - /* fill in Linear accel values */ - fillLinearAccel(list); - } else { - /* no 9-axis sensors, zero fill that part of the list */ - numsensors = 3; - memset(list + 3, 0, 4 * sizeof(struct sensor_t)); - } - - return numsensors; -} - -void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) -{ - VFUNC_LOG; - - if (accel) { - if(accel != NULL && strcmp(accel, "BMA250") == 0) { - list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; - list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA250_POWER; - list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; - return; - } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) { - list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; - list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; - list[Accelerometer].power = ACCEL_MPU6050_POWER; - list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY; - return; - } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) { - list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; - list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; - list[Accelerometer].power = ACCEL_MPU6500_POWER; - list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; - - return; - } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) { - list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE; - list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION; - list[Accelerometer].power = ACCEL_MPU9150_POWER; - list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY; - return; - } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { - list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; - list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA250_POWER; - list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; - return; - } - } - - LOGE("HAL:unknown accel id %s -- " - "params default to bma250 and might be wrong.", - accel); - list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; - list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA250_POWER; - list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; -} - -void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) -{ - VFUNC_LOG; - - if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) { - list[Gyro].maxRange = GYRO_MPU3050_RANGE; - list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; - list[Gyro].power = GYRO_MPU3050_POWER; - list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; - } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) { - list[Gyro].maxRange = GYRO_MPU6050_RANGE; - list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; - list[Gyro].power = GYRO_MPU6050_POWER; - list[Gyro].minDelay = GYRO_MPU6050_MINDELAY; - } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) { - list[Gyro].maxRange = GYRO_MPU6500_RANGE; - list[Gyro].resolution = GYRO_MPU6500_RESOLUTION; - list[Gyro].power = GYRO_MPU6500_POWER; - list[Gyro].minDelay = GYRO_MPU6500_MINDELAY; - } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) { - list[Gyro].maxRange = GYRO_MPU9150_RANGE; - list[Gyro].resolution = GYRO_MPU9150_RESOLUTION; - list[Gyro].power = GYRO_MPU9150_POWER; - list[Gyro].minDelay = GYRO_MPU9150_MINDELAY; - } else { - LOGE("HAL:unknown gyro id -- gyro params will be wrong."); - LOGE("HAL:default to use mpu3050 params"); - list[Gyro].maxRange = GYRO_MPU3050_RANGE; - list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; - list[Gyro].power = GYRO_MPU3050_POWER; - list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; - } - - list[RawGyro].maxRange = list[Gyro].maxRange; - list[RawGyro].resolution = list[Gyro].resolution; - list[RawGyro].power = list[Gyro].power; - list[RawGyro].minDelay = list[Gyro].minDelay; - - return; -} - -/* fillRV depends on values of accel and compass in the list */ -void MPLSensor::fillRV(struct sensor_t *list) -{ - VFUNC_LOG; - - /* compute power on the fly */ - list[RotationVector].power = list[Gyro].power + - list[Accelerometer].power + - list[MagneticField].power; - list[RotationVector].resolution = .00001; - list[RotationVector].maxRange = 1.0; - list[RotationVector].minDelay = 5000; - - return; -} - -void MPLSensor::fillOrientation(struct sensor_t *list) -{ - VFUNC_LOG; - - list[Orientation].power = list[Gyro].power + - list[Accelerometer].power + - list[MagneticField].power; - list[Orientation].resolution = .00001; - list[Orientation].maxRange = 360.0; - list[Orientation].minDelay = 5000; - - return; -} - -void MPLSensor::fillGravity( struct sensor_t *list) -{ - VFUNC_LOG; - - list[Gravity].power = list[Gyro].power + - list[Accelerometer].power + - list[MagneticField].power; - list[Gravity].resolution = .00001; - list[Gravity].maxRange = 9.81; - list[Gravity].minDelay = 5000; - - return; -} - -void MPLSensor::fillLinearAccel(struct sensor_t *list) -{ - VFUNC_LOG; - - list[LinearAccel].power = list[Gyro].power + - list[Accelerometer].power + - list[MagneticField].power; - list[LinearAccel].resolution = list[Accelerometer].resolution; - list[LinearAccel].maxRange = list[Accelerometer].maxRange; - list[LinearAccel].minDelay = 5000; - - return; -} - -int MPLSensor::inv_init_sysfs_attributes(void) -{ - VFUNC_LOG; - - unsigned char i; - char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN]; - char *sptr; - char **dptr; - - sysfs_names_ptr = - (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); - sptr = sysfs_names_ptr; - if (sptr == NULL) { - LOGE("HAL:couldn't alloc mem for sysfs paths"); - return -1; - } - - dptr = (char**)&mpu; - i = 0; - do { - *dptr++ = sptr; - sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); - } while (++i < MAX_SYSFS_ATTRB); - - // get proper (in absolute/relative) IIO path & build MPU's sysfs paths - // inv_get_sysfs_abs_path(sysfs_path); - if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) { - ALOGE("MPLSensor failed get sysfs path"); - return -1; - } - - if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) { - ALOGE("MPLSensor failed get iio trigger path"); - return -1; - } - - sprintf(mpu.key, "%s%s", sysfs_path, "/key"); - sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); - sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length"); - sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); - sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en"); - sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name"); - sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger"); - - sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware"); - sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded"); - sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on"); - sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on"); - sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on"); - sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate"); - sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on"); - - // TODO: for self test - sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test"); - - sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); - sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); - sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); - sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix"); - sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en"); - sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en"); - sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en"); - - sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); - sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); - sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); - - -#ifndef THIRD_PARTY_ACCEL //MPUxxxx - sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); - // TODO: for bias settings - sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias"); -#endif - - sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en"); - sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en"); - sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en"); - - sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on"); - sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en"); - sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en"); - sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en"); - sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en"); - - sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on"); - sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation"); - -#if SYSFS_VERBOSE - // test print sysfs paths - dptr = (char**)&mpu; - for (i = 0; i < MAX_SYSFS_ATTRB; i++) { - LOGE("HAL:sysfs path: %s", *dptr++); - } -#endif - return 0; -} - -/* TODO: stop manually testing/using 0 and 1 instead of - * false and true, but just use 0 and non-0. - * This allows passing 0 and non-0 ints around instead of - * having to convert to 1 and test against 1. - */ -bool MPLSensor::isMpu3050() -{ - return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"); -} - -int MPLSensor::isLowPowerQuatEnabled() -{ -#ifdef ENABLE_LP_QUAT_FEAT - return !isMpu3050(); -#else - return 0; -#endif -} - -int MPLSensor::isDmpDisplayOrientationOn() -{ -#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT - return !isMpu3050(); -#else - return 0; -#endif -} diff --git a/60xx/libsensors_iio/MPLSensor.h b/60xx/libsensors_iio/MPLSensor.h deleted file mode 100644 index 4698f33..0000000 --- a/60xx/libsensors_iio/MPLSensor.h +++ /dev/null @@ -1,346 +0,0 @@ -/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_MPL_SENSOR_H
-#define ANDROID_MPL_SENSOR_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-#include <poll.h>
-#include <utils/Vector.h>
-#include <utils/KeyedVector.h>
-#include "sensors.h"
-#include "SensorBase.h"
-#include "InputEventReader.h"
-
-#ifdef INVENSENSE_COMPASS_CAL
-
-#ifdef COMPASS_YAS53x
-#pragma message "unified HAL for YAS53x"
-#include "CompassSensor.IIO.primary.h"
-#elif COMPASS_AMI306
-#pragma message "unified HAL for AMI306"
-#include "CompassSensor.IIO.primary.h"
-#else
-#pragma message "unified HAL for MPU9150"
-#include "CompassSensor.IIO.9150.h"
-#endif
-#else
-#pragma message "unified HAL for AKM"
-#include "CompassSensor.AKM.h"
-#endif
-
-/*****************************************************************************/
-/* Sensors Enable/Disable Mask
- *****************************************************************************/
-#define MAX_CHIP_ID_LEN (20)
-
-#define INV_THREE_AXIS_GYRO (0x000F)
-#define INV_THREE_AXIS_ACCEL (0x0070)
-#define INV_THREE_AXIS_COMPASS (0x0380)
-#define INV_ALL_SENSORS (0x7FFF)
-
-#ifdef INVENSENSE_COMPASS_CAL
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
- | INV_THREE_AXIS_COMPASS \
- | INV_THREE_AXIS_GYRO)
-#else
-#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \
- | INV_THREE_AXIS_COMPASS \
- | INV_THREE_AXIS_GYRO)
-#endif
-
-// bit mask of current active features (mFeatureActiveMask)
-#define INV_COMPASS_CAL 0x01
-#define INV_COMPASS_FIT 0x02
-#define INV_DMP_QUATERNION 0x04
-#define INV_DMP_DISPL_ORIENTATION 0x08
-
-/* Uncomment to enable Low Power Quaternion */
-#define ENABLE_LP_QUAT_FEAT
-
-/* Uncomment to enable DMP display orientation
- (within the HAL, see below for Java framework) */
-// #define ENABLE_DMP_DISPL_ORIENT_FEAT
-
-#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
-/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION
- sensor type (DMP screen orientation) to the Java framework.
- NOTE:
- need Invensense customized
- 'hardware/libhardware/include/hardware/sensors.h' to compile correctly.
- NOTE:
- need Invensense java framework changes to:
- 'frameworks/base/core/java/android/view/WindowOrientationListener.java'
- 'frameworks/base/core/java/android/hardware/Sensor.java'
- 'frameworks/base/core/java/android/hardware/SensorEvent.java'
- for the 'Auto-rotate screen' to use this feature.
-*/
-#define ENABLE_DMP_SCREEN_AUTO_ROTATION
-#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly"
-#endif
-
-int isDmpScreenAutoRotationEnabled()
-{
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
- return 1;
-#else
- return 0;
-#endif
-}
-
-int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL;
-/*****************************************************************************/
-/** MPLSensor implementation which fits into the HAL example for crespo provided
- * by Google.
- * WARNING: there may only be one instance of MPLSensor, ever.
- */
-
-class MPLSensor: public SensorBase
-{
- typedef int (MPLSensor::*hfunc_t)(sensors_event_t*);
-
-public:
-
- enum {
- Gyro = 0,
- RawGyro,
- Accelerometer,
- MagneticField,
- Orientation,
- RotationVector,
- LinearAccel,
- Gravity,
- numSensors
- };
-
- MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0);
- virtual ~MPLSensor();
-
- virtual int setDelay(int32_t handle, int64_t ns);
- virtual int enable(int32_t handle, int enabled);
- int32_t getEnableMask() { return mEnabled; }
-
- virtual int readEvents(sensors_event_t *data, int count);
- virtual int getFd() const;
- virtual int getAccelFd() const;
- virtual int getCompassFd() const;
- virtual int getPollTime();
- virtual bool hasPendingEvents() const;
- virtual void sleepEvent();
- virtual void wakeEvent();
- int populateSensorList(struct sensor_t *list, int len);
- void cbProcData();
-
- //static pointer to the object that will handle callbacks
- static MPLSensor* gMPLSensor;
-
- //AKM HAL Integration
- //void set_compass(long ready, long x, long y, long z, long accuracy);
- int executeOnData(sensors_event_t* data, int count);
- int readAccelEvents(sensors_event_t* data, int count);
- int readCompassEvents(sensors_event_t* data, int count);
-
- int turnOffAccelFifo();
- int enableDmpOrientation(int);
- int dmpOrientHandler(int);
- int readDmpOrientEvents(sensors_event_t* data, int count);
- int getDmpOrientFd();
- int openDmpOrientFd();
- int closeDmpOrientFd();
-
- int getDmpRate(int64_t *);
- int checkDMPOrientation();
-
-protected:
- CompassSensor *mCompassSensor;
-
- int gyroHandler(sensors_event_t *data);
- int rawGyroHandler(sensors_event_t *data);
- int accelHandler(sensors_event_t *data);
- int compassHandler(sensors_event_t *data);
- int rvHandler(sensors_event_t *data);
- int laHandler(sensors_event_t *data);
- int gravHandler(sensors_event_t *data);
- int orienHandler(sensors_event_t *data);
- void calcOrientationSensor(float *Rx, float *Val);
- virtual int update_delay();
-
- void inv_set_device_properties();
- int inv_constructor_init();
- int inv_constructor_default_enable();
- int setGyroInitialState();
- int setAccelInitialState();
- int masterEnable(int en);
- int onPower(int en);
- int enableLPQuaternion(int);
- int enableQuaternionData(int);
- int onDMP(int);
- int enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int));
- int enableGyro(int en);
- int enableAccel(int en);
- int enableCompass(int en);
- void computeLocalSensorMask(int enabled_sensors);
- int enableSensors(unsigned long sensors, int en, uint32_t changed);
- int inv_read_gyro_buffer(int fd, short *data, long long *timestamp);
- int inv_float_to_q16(float *fdata, long *ldata);
- int inv_long_to_q16(long *fdata, long *ldata);
- int inv_float_to_round(float *fdata, long *ldata);
- int inv_float_to_round2(float *fdata, short *sdata);
- int inv_read_temperature(long long *data);
- int inv_read_dmp_state(int fd);
- int inv_read_sensor_bias(int fd, long *data);
- void inv_get_sensors_orientation(void);
- int inv_init_sysfs_attributes(void);
-#ifdef COMPASS_YAS53x
- int resetCompass(void);
-#endif
- void setCompassDelay(int64_t ns);
- void enable_iio_sysfs(void);
- int enableTap(int);
- int enableFlick(int);
- int enablePedometer(int);
- int checkLPQuaternion();
-
- int mNewData; // flag indicating that the MPL calculated new output values
- int mDmpStarted;
- long mMasterSensorMask;
- long mLocalSensorMask;
- int mPollTime;
- bool mHaveGoodMpuCal; // flag indicating that the cal file can be written
- int mGyroAccuracy; // value indicating the quality of the gyro calibr.
- int mAccelAccuracy; // value indicating the quality of the accel calibr.
- int mCompassAccuracy; // value indicating the quality of the compass calibr.
- struct pollfd mPollFds[5];
- int mSampleCount;
- pthread_mutex_t mMplMutex;
- pthread_mutex_t mHALMutex;
-
- char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH];
-
- int iio_fd;
- int accel_fd;
- int mpufifo_fd;
- int gyro_temperature_fd;
- int dmp_orient_fd;
-
- int mDmpOrientationEnabled;
-
-
- uint32_t mEnabled;
- uint32_t mOldEnabledMask;
- sensors_event_t mPendingEvents[numSensors];
- int64_t mDelays[numSensors];
- hfunc_t mHandlers[numSensors];
- short mCachedGyroData[3];
- long mCachedAccelData[3];
- long mCachedCompassData[3];
- long mCachedQuaternionData[4];
- android::KeyedVector<int, int> mIrqFds;
-
- InputEventCircularReader mAccelInputReader;
- InputEventCircularReader mGyroInputReader;
-
- bool mFirstRead;
- short mTempScale;
- short mTempOffset;
- int64_t mTempCurrentTime;
- int mAccelScale;
-
- uint32_t mPendingMask;
- unsigned long mSensorMask;
-
- char chip_ID[MAX_CHIP_ID_LEN];
-
- signed char mGyroOrientation[9];
- signed char mAccelOrientation[9];
-
- int64_t mSensorTimestamp;
- int64_t mCompassTimestamp;
-
- struct sysfs_attrbs {
- char *chip_enable;
- char *power_state;
- char *dmp_firmware;
- char *firmware_loaded;
- char *dmp_on;
- char *dmp_int_on;
- char *dmp_event_int_on;
- char *dmp_output_rate;
- char *tap_on;
- char *key;
- char *self_test;
- char *temperature;
-
- char *gyro_enable;
- char *gyro_fifo_rate;
- char *gyro_orient;
- char *gyro_x_fifo_enable;
- char *gyro_y_fifo_enable;
- char *gyro_z_fifo_enable;
-
- char *accel_enable;
- char *accel_fifo_rate;
- char *accel_fsr;
- char *accel_bias;
- char *accel_orient;
- char *accel_x_fifo_enable;
- char *accel_y_fifo_enable;
- char *accel_z_fifo_enable;
-
- char *quaternion_on;
- char *in_quat_r_en;
- char *in_quat_x_en;
- char *in_quat_y_en;
- char *in_quat_z_en;
-
- char *in_timestamp_en;
- char *trigger_name;
- char *current_trigger;
- char *buffer_length;
-
- char *display_orientation_on;
- char *event_display_orientation;
- } mpu;
-
- char *sysfs_names_ptr;
- int mFeatureActiveMask;
-
-private:
- /* added for dynamic get sensor list */
- void fillAccel(const char* accel, struct sensor_t *list);
- void fillGyro(const char* gyro, struct sensor_t *list);
- void fillRV(struct sensor_t *list);
- void fillOrientation(struct sensor_t *list);
- void fillGravity(struct sensor_t *list);
- void fillLinearAccel(struct sensor_t *list);
- void storeCalibration();
- void loadDMP();
- bool isMpu3050();
- int isLowPowerQuatEnabled();
- int isDmpDisplayOrientationOn();
-
-
-};
-
-extern "C" {
- void setCallbackObject(MPLSensor*);
- MPLSensor *getCallbackObject();
-}
-
-#endif // ANDROID_MPL_SENSOR_H
diff --git a/60xx/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp deleted file mode 100644 index 9773562..0000000 --- a/60xx/libsensors_iio/MPLSupport.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/* -* Copyright (C) 2012 Invensense, Inc. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*/ - -#define LOG_NDEBUG 0 - -#include <MPLSupport.h> -#include <string.h> -#include <stdio.h> -#include <fcntl.h> - -#include "log.h" -#include "SensorBase.h" - -#include "ml_sysfs_helper.h" -#include "local_log_def.h" - -int64_t getTimestamp() -{ - struct timespec t; - t.tv_sec = t.tv_nsec = 0; - clock_gettime(CLOCK_MONOTONIC, &t); - return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec; -} - -int64_t timevalToNano(timeval const& t) { - return t.tv_sec * 1000000000LL + t.tv_usec * 1000; -} - -int inv_read_data(char *fname, long *data) -{ - VFUNC_LOG; - - char buf[sizeof(long) * 4]; - int count, fd; - - fd = open(fname, O_RDONLY); - if(fd < 0) { - LOGE("HAL:Error opening %s", fname); - return -1; - } - memset(buf, 0, sizeof(buf)); - count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 1) { - close(fd); - return -1; - } else { - count = sscanf(buf, "%ld", data); - if(count) - LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data); - } - close(fd); - - return 0; -} - -/* This one DOES NOT close FDs for you */ -int read_attribute_sensor(int fd, char* data, unsigned int size) -{ - VFUNC_LOG; - - int count = 0; - if (fd > 0) { - count = pread(fd, data, size, 0); - if(count < 1) { - LOGE("HAL:read fails with error code=%d", count); - } - } - return count; -} - -/** - * @brief Enable a sensor through the sysfs file descriptor - * provided. - * @note this function one closes FD after the write - * @param fd - * the file descriptor to write into - * @param en - * the value to write, typically 1 or 0 - * @return the errno whenever applicable. - */ -int enable_sysfs_sensor(int fd, int en) -{ - VFUNC_LOG; - - int nb; - int err = 0; - - char c = en ? '1' : '0'; - nb = write(fd, &c, 1); - - if (nb <= 0) { - err = errno; - LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)", - c, nb, strerror(err), err); - } - close(fd); - - - return err; -} - -/* This one closes FDs for you */ -int write_attribute_sensor(int fd, long data) -{ - VFUNC_LOG; - - int num_b = 0; - - if (fd >= 0) { - char buf[80]; - sprintf(buf, "%ld", data); - num_b = write(fd, buf, strlen(buf) + 1); - if (num_b <= 0) { - int err = errno; - LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err); - } else { - LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data); - } - close(fd); - } - - return num_b; -} - -int read_sysfs_int(char *filename, int *var) -{ - int res=0; - FILE *sysfsfp; - - sysfsfp = fopen(filename, "r"); - if (sysfsfp != NULL) { - if (fscanf(sysfsfp, "%d\n", var) < 1) { - LOGE("HAL:ERR failed to read an int from %s.", filename); - res = -EINVAL; - } - fclose(sysfsfp); - } else { - res = -errno; - LOGE("HAL:ERR open file %s to read with error %d", filename, res); - } - return res; -} - -int write_sysfs_int(char *filename, int var) -{ - int res = 0; - FILE *sysfsfp; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - var, filename, getTimestamp()); - sysfsfp = fopen(filename, "w"); - if (sysfsfp == NULL) { - res = -errno; - LOGE("HAL:ERR open file %s to write with error %d", filename, res); - return res; - } - int fpres, fcres = 0; - fpres = fprintf(sysfsfp, "%d\n", var); - /* fprintf() can succeed because it never actually writes to the - * underlying sysfs node. - */ - if (fpres < 0) { - res = -errno; - fclose(sysfsfp); - } else { - fcres = fclose(sysfsfp); - /* Check for errors from: fflush(), write(), and close() */ - if (fcres < 0) { - res = -errno; - } - } - if (fpres < 0 || fcres < 0) { - LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d", - var, filename, res, fpres, fcres); - } - return res; -} diff --git a/60xx/libsensors_iio/MPLSupport.h b/60xx/libsensors_iio/MPLSupport.h deleted file mode 100644 index 98e4497..0000000 --- a/60xx/libsensors_iio/MPLSupport.h +++ /dev/null @@ -1,33 +0,0 @@ -/* -* Copyright (C) 2012 Invensense, Inc. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*/ - -#ifndef ANDROID_MPL_SUPPORT_H -#define ANDROID_MPL_SUPPORT_H - -#include <stdint.h> -#include <time.h> - -int64_t getTimestamp(); -int64_t timevalToNano(timeval const& t); - -int inv_read_data(char *fname, long *data); -int read_attribute_sensor(int fd, char* data, unsigned int size); -int enable_sysfs_sensor(int fd, int en); -int write_attribute_sensor(int fd, long data); -int read_sysfs_int(char*, int*); -int write_sysfs_int(char*, int); - -#endif // ANDROID_MPL_SUPPORT_H diff --git a/60xx/libsensors_iio/SensorBase.cpp b/60xx/libsensors_iio/SensorBase.cpp deleted file mode 100644 index 587aaf5..0000000 --- a/60xx/libsensors_iio/SensorBase.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <dirent.h>
-#include <string.h>
-#include <sys/select.h>
-#include <cutils/log.h>
-#include <linux/input.h>
-
-#include "SensorBase.h"
-#include "local_log_def.h"
-
-/*****************************************************************************/
-
-SensorBase::SensorBase(const char* dev_name,
- const char* data_name) : dev_name(dev_name),
- data_name(data_name),
- dev_fd(-1),
- data_fd(-1)
-{
- if (data_name) {
- data_fd = openInput(data_name);
- }
-}
-
-SensorBase::~SensorBase()
-{
- if (data_fd >= 0) {
- close(data_fd);
- }
- if (dev_fd >= 0) {
- close(dev_fd);
- }
-}
-
-int SensorBase::open_device()
-{
- if (dev_fd<0 && dev_name) {
- dev_fd = open(dev_name, O_RDONLY);
- LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno));
- }
- return 0;
-}
-
-int SensorBase::close_device()
-{
- if (dev_fd >= 0) {
- close(dev_fd);
- dev_fd = -1;
- }
- return 0;
-}
-
-int SensorBase::getFd() const
-{
- if (!data_name) {
- return dev_fd;
- }
- return data_fd;
-}
-
-int SensorBase::setDelay(int32_t /*handle*/, int64_t /*ns*/)
-{
- return 0;
-}
-
-bool SensorBase::hasPendingEvents() const
-{
- return false;
-}
-
-int SensorBase::openInput(const char *inputName)
-{
- int fd = -1;
- const char *dirname = "/dev/input";
- char devname[PATH_MAX];
- char *filename;
- DIR *dir;
- struct dirent *de;
- dir = opendir(dirname);
- if(dir == NULL)
- return -1;
- strcpy(devname, dirname);
- filename = devname + strlen(devname);
- *filename++ = '/';
- while((de = readdir(dir))) {
- if(de->d_name[0] == '.' &&
- (de->d_name[1] == '\0' ||
- (de->d_name[1] == '.' && de->d_name[2] == '\0')))
- continue;
- strcpy(filename, de->d_name);
- fd = open(devname, O_RDONLY);
- LOGV_IF(EXTRA_VERBOSE, "path open %s", devname);
- LOGI("path open %s", devname);
- if (fd >= 0) {
- char name[80];
- if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
- name[0] = '\0';
- }
- LOGV_IF(EXTRA_VERBOSE, "name read %s", name);
- if (!strcmp(name, inputName)) {
- strcpy(input_name, filename);
- break;
- } else {
- close(fd);
- fd = -1;
- }
- }
- }
- closedir(dir);
- LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName);
- return fd;
-}
-
-int SensorBase::enable(int32_t /*handle*/, int /*enabled*/)
-{
- return 0;
-}
diff --git a/60xx/libsensors_iio/SensorBase.h b/60xx/libsensors_iio/SensorBase.h deleted file mode 100644 index fef47c7..0000000 --- a/60xx/libsensors_iio/SensorBase.h +++ /dev/null @@ -1,61 +0,0 @@ -/* -* Copyright (C) 2012 Invensense, Inc. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*/ - -#ifndef ANDROID_SENSOR_BASE_H -#define ANDROID_SENSOR_BASE_H - -#include <stdint.h> -#include <errno.h> -#include <sys/cdefs.h> -#include <sys/types.h> - -#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember)) - -#define MAX_SYSFS_NAME_LEN (100) -#define IIO_BUFFER_LENGTH (480) - -/*****************************************************************************/ - -struct sensors_event_t; - -class SensorBase { -protected: - const char *dev_name; - const char *data_name; - char input_name[PATH_MAX]; - int dev_fd; - int data_fd; - - int openInput(const char* inputName); - - int open_device(); - int close_device(); - -public: - SensorBase(const char* dev_name, const char* data_name); - - virtual ~SensorBase(); - - virtual int readEvents(sensors_event_t* data, int count) = 0; - virtual bool hasPendingEvents() const; - virtual int getFd() const; - virtual int setDelay(int32_t handle, int64_t ns); - virtual int enable(int32_t handle, int enabled); -}; - -/*****************************************************************************/ - -#endif // ANDROID_SENSOR_BASE_H diff --git a/60xx/libsensors_iio/libmllite.so b/60xx/libsensors_iio/libmllite.so Binary files differdeleted file mode 100755 index 9bdd5bc..0000000 --- a/60xx/libsensors_iio/libmllite.so +++ /dev/null diff --git a/60xx/libsensors_iio/libmplmpu.so b/60xx/libsensors_iio/libmplmpu.so Binary files differdeleted file mode 100644 index 205ab9a..0000000 --- a/60xx/libsensors_iio/libmplmpu.so +++ /dev/null diff --git a/60xx/libsensors_iio/local_log_def.h b/60xx/libsensors_iio/local_log_def.h deleted file mode 100644 index 9135992..0000000 --- a/60xx/libsensors_iio/local_log_def.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef LOCAL_LOG_DEF_H
-#define LOCAL_LOG_DEF_H
-
-/* Log enablers, each of these independent */
-
-#define PROCESS_VERBOSE (0) /* process log messages */
-#define EXTRA_VERBOSE (0) /* verbose log messages */
-#define SYSFS_VERBOSE (0) /* log sysfs interactions as cat/echo for repro
- purpose on a shell */
-#define FUNC_ENTRY (0) /* log entry in all one-time functions */
-
-/* Note that enabling this logs may affect performance */
-#define HANDLER_ENTRY (0) /* log entry in all handler functions */
-#define ENG_VERBOSE (0) /* log some a lot more info about the internals */
-#define INPUT_DATA (0) /* log the data input from the events */
-#define HANDLER_DATA (0) /* log the data fetched from the handlers */
-
-#if defined ANDROID_JELLYBEAN
-#define LOGV ALOGV
-#define LOGV_IF ALOGV_IF
-#define LOGD ALOGD
-#define LOGD_IF ALOGD_IF
-#define LOGI ALOGI
-#define LOGI_IF ALOGI_IF
-#define LOGW ALOGW
-#define LOGW_IF ALOGW_IF
-#define LOGE ALOGE
-#define LOGE_IF ALOGE_IF
-#define IF_LOGV IF_ALOGV
-#define IF_LOGD IF_ALOGD
-#define IF_LOGI IF_ALOGI
-#define IF_LOGW IF_ALOGW
-#define IF_LOGE IF_ALOGE
-#define LOG_ASSERT ALOG_ASSERT
-#define LOG ALOG
-#define IF_LOG IF_ALOG
-#else
-#warning "build for ICS or earlier version"
-#endif
-
-
-#define FUNC_LOG \
- LOGV("%s", __PRETTY_FUNCTION__)
-#define VFUNC_LOG \
- LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__)
-#define VHANDLER_LOG \
- LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__)
-
-#endif /*ifndef LOCAL_LOG_DEF_H */
diff --git a/60xx/libsensors_iio/sensor_params.h b/60xx/libsensors_iio/sensor_params.h deleted file mode 100644 index eef0b3b..0000000 --- a/60xx/libsensors_iio/sensor_params.h +++ /dev/null @@ -1,194 +0,0 @@ -/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef INV_SENSOR_PARAMS_H
-#define INV_SENSOR_PARAMS_H
-
-/* Physical parameters of the sensors supported by Invensense MPL */
-#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV)
-#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA)
-#define SENSORS_GRAVITY_HANDLE (ID_GR)
-#define SENSORS_GYROSCOPE_HANDLE (ID_GY)
-#define SENSORS_RAW_GYROSCOPE_HANDLE (ID_RG)
-#define SENSORS_ACCELERATION_HANDLE (ID_A)
-#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M)
-#define SENSORS_ORIENTATION_HANDLE (ID_O)
-#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO)
-
-/******************************************/
-//MPU9250 INV_COMPASS
-#define COMPASS_MPU9250_RANGE (9830.f)
-#define COMPASS_MPU9250_RESOLUTION (0.15f)
-#define COMPASS_MPU9250_POWER (10.f)
-#define COMPASS_MPU9250_MINDELAY (10000)
-//MPU9150 INV_COMPASS
-#define COMPASS_MPU9150_RANGE (9830.f)
-#define COMPASS_MPU9150_RESOLUTION (0.285f)
-#define COMPASS_MPU9150_POWER (10.f)
-#define COMPASS_MPU9150_MINDELAY (10000)
-//COMPASS_ID_AK8975
-#define COMPASS_AKM8975_RANGE (9830.f)
-#define COMPASS_AKM8975_RESOLUTION (0.285f)
-#define COMPASS_AKM8975_POWER (10.f)
-#define COMPASS_AKM8975_MINDELAY (10000)
-//COMPASS_ID_AK8963C
-#define COMPASS_AKM8963_RANGE (9830.f)
-#define COMPASS_AKM8963_RESOLUTION (0.15f)
-#define COMPASS_AKM8963_POWER (10.f)
-#define COMPASS_AKM8963_MINDELAY (10000)
-//COMPASS_ID_AMI30X
-#define COMPASS_AMI30X_RANGE (5461.f)
-#define COMPASS_AMI30X_RESOLUTION (0.9f)
-#define COMPASS_AMI30X_POWER (0.15f)
-//COMPASS_ID_AMI306
-#define COMPASS_AMI306_RANGE (5461.f)
-#define COMPASS_AMI306_RESOLUTION (0.9f)
-#define COMPASS_AMI306_POWER (0.15f)
-#define COMPASS_AMI306_MINDELAY (10000)
-//COMPASS_ID_YAS529
-#define COMPASS_YAS529_RANGE (19660.f)
-#define COMPASS_YAS529_RESOLUTION (0.012f)
-#define COMPASS_YAS529_POWER (4.f)
-//COMPASS_ID_YAS53x
-#define COMPASS_YAS53x_RANGE (8001.f)
-#define COMPASS_YAS53x_RESOLUTION (0.012f)
-#define COMPASS_YAS53x_POWER (4.f)
-#define COMPASS_YAS53x_MINDELAY (10000)
-//COMPASS_ID_HMC5883
-#define COMPASS_HMC5883_RANGE (10673.f)
-#define COMPASS_HMC5883_RESOLUTION (10.f)
-#define COMPASS_HMC5883_POWER (0.24f)
-//COMPASS_ID_LSM303DLH
-#define COMPASS_LSM303DLH_RANGE (10240.f)
-#define COMPASS_LSM303DLH_RESOLUTION (1.f)
-#define COMPASS_LSM303DLH_POWER (1.f)
-//COMPASS_ID_LSM303DLM
-#define COMPASS_LSM303DLM_RANGE (10240.f)
-#define COMPASS_LSM303DLM_RESOLUTION (1.f)
-#define COMPASS_LSM303DLM_POWER (1.f)
-//COMPASS_ID_MMC314X
-#define COMPASS_MMC314X_RANGE (400.f)
-#define COMPASS_MMC314X_RESOLUTION (2.f)
-#define COMPASS_MMC314X_POWER (0.55f)
-//COMPASS_ID_HSCDTD002B
-#define COMPASS_HSCDTD002B_RANGE (9830.f)
-#define COMPASS_HSCDTD002B_RESOLUTION (1.f)
-#define COMPASS_HSCDTD002B_POWER (1.f)
-//COMPASS_ID_HSCDTD004A
-#define COMPASS_HSCDTD004A_RANGE (9830.f)
-#define COMPASS_HSCDTD004A_RESOLUTION (1.f)
-#define COMPASS_HSCDTD004A_POWER (1.f)
-/*******************************************/
-//ACCEL_ID_MPU6500
-#define ACCEL_MPU6500_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU6500_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU6500_POWER (0.f)
-#define ACCEL_MPU6500_MINDELAY (1000)
-//ACCEL_ID_MPU9250
-#define ACCEL_MPU9250_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU9250_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU9250_POWER (0.f)
-#define ACCEL_MPU9250_MINDELAY (1000)
-//ACCEL_ID_MPU9150
-#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU9150_POWER (0.f)
-#define ACCEL_MPU9150_MINDELAY (1000)
-//ACCEL_ID_LIS331
-#define ACCEL_LIS331_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LIS331_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LIS331_POWER (1.f)
-//ACCEL_ID_LSM303DLX
-#define ACCEL_LSM303DLX_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LSM303DLX_POWER (1.f)
-//ACCEL_ID_LIS3DH
-#define ACCEL_LIS3DH_RANGE (2.48f * GRAVITY_EARTH)
-#define ACCEL_LIS3DH_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_LIS3DH_POWER (1.f)
-//ACCEL_ID_KXSD9
-#define ACCEL_KXSD9_RANGE (2.5006f * GRAVITY_EARTH)
-#define ACCEL_KXSD9_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_KXSD9_POWER (1.f)
-//ACCEL_ID_KXTF9
-#define ACCEL_KXTF9_RANGE (1.f * GRAVITY_EARTH)
-#define ACCEL_KXTF9_RESOLUTION (0.033f * GRAVITY_EARTH)
-#define ACCEL_KXTF9_POWER (0.35f)
-//ACCEL_ID_BMA150
-#define ACCEL_BMA150_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA150_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_BMA150_POWER (0.2f)
-//ACCEL_ID_BMA222
-#define ACCEL_BMA222_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA222_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_BMA222_POWER (0.1f)
-//ACCEL_ID_BMA250
-#define ACCEL_BMA250_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_BMA250_RESOLUTION (0.00391f * GRAVITY_EARTH)
-#define ACCEL_BMA250_POWER (0.139f)
-#define ACCEL_BMA250_MINDELAY (1000)
-//ACCEL_ID_ADXL34X
-#define ACCEL_ADXL34X_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_ADXL34X_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_ADXL34X_POWER (1.f)
-//ACCEL_ID_MMA8450
-#define ACCEL_MMA8450_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MMA8450_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_MMA8450_POWER (1.0f)
-//ACCEL_ID_MMA845X
-#define ACCEL_MMA845X_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MMA845X_RESOLUTION (0.001f * GRAVITY_EARTH)
-#define ACCEL_MMA845X_POWER (1.f)
-//ACCEL_ID_MPU6050
-#define ACCEL_MPU6050_RANGE (2.f * GRAVITY_EARTH)
-#define ACCEL_MPU6050_RESOLUTION (0.004f * GRAVITY_EARTH)
-#define ACCEL_MPU6050_POWER (0.f)
-#define ACCEL_MPU6050_MINDELAY (5000)
-/******************************************/
-//GYRO MPU3050
-#define RAD_P_DEG (3.14159f / 180.f)
-#define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU3050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU3050_POWER (6.1f)
-#define GYRO_MPU3050_MINDELAY (1000)
-//GYRO MPU6050
-#define GYRO_MPU6050_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU6050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU6050_POWER (5.5f)
-#define GYRO_MPU6050_MINDELAY (5000)
-//GYRO MPU9150
-#define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU9150_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU9150_POWER (5.5f)
-#define GYRO_MPU9150_MINDELAY (1000)
-//GYRO MPU9250
-#define GYRO_MPU9250_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU9250_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU9250_POWER (5.5f)
-#define GYRO_MPU9250_MINDELAY (1000)
-//GYRO MPU6500
-#define GYRO_MPU6500_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_MPU6500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_MPU6500_POWER (5.5f)
-#define GYRO_MPU6500_MINDELAY (1000)
-//GYRO ITG3500
-#define GYRO_ITG3500_RANGE (2000.f * RAD_P_DEG)
-#define GYRO_ITG3500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG)
-#define GYRO_ITG3500_POWER (5.5f)
-#define GYRO_ITG3500_MINDELAY (1000)
-
-#endif /* INV_SENSOR_PARAMS_H */
-
diff --git a/60xx/libsensors_iio/sensors.h b/60xx/libsensors_iio/sensors.h deleted file mode 100644 index f698fc5..0000000 --- a/60xx/libsensors_iio/sensors.h +++ /dev/null @@ -1,97 +0,0 @@ -/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#ifndef ANDROID_SENSORS_H
-#define ANDROID_SENSORS_H
-
-#include <stdint.h>
-#include <errno.h>
-#include <sys/cdefs.h>
-#include <sys/types.h>
-
-#include <linux/input.h>
-
-#include <hardware/hardware.h>
-#include <hardware/sensors.h>
-
-__BEGIN_DECLS
-
-/*****************************************************************************/
-
-#ifndef ARRAY_SIZE
-#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
-#endif
-
-enum {
- ID_GY = 0,
- ID_RG,
- ID_A,
- ID_M,
- ID_O,
- ID_RV,
- ID_LA,
- ID_GR,
- ID_SO
-};
-
-/*****************************************************************************/
-
-/*
- * The SENSORS Module
- */
-
-/* ITG3500 */
-#define EVENT_TYPE_GYRO_X REL_X
-#define EVENT_TYPE_GYRO_Y REL_Y
-#define EVENT_TYPE_GYRO_Z REL_Z
-/* MPU6050 MPU9150 */
-#define EVENT_TYPE_IACCEL_X REL_RX
-#define EVENT_TYPE_IACCEL_Y REL_RY
-#define EVENT_TYPE_IACCEL_Z REL_RZ
-/* MPU6050 MPU9150 */
-#define EVENT_TYPE_ICOMPASS_X REL_X
-#define EVENT_TYPE_ICOMPASS_Y REL_Y
-#define EVENT_TYPE_ICOMPASS_Z REL_Z
-/* MPUxxxx */
-#define EVENT_TYPE_TIMESTAMP_HI REL_MISC
-#define EVENT_TYPE_TIMESTAMP_LO REL_WHEEL
-
-/* Accel BMA250 */
-#define EVENT_TYPE_ACCEL_X ABS_X
-#define EVENT_TYPE_ACCEL_Y ABS_Y
-#define EVENT_TYPE_ACCEL_Z ABS_Z
-#define LSG (1000.0f)
-
-// conversion of acceleration data to SI units (m/s^2)
-#define RANGE_A (4*GRAVITY_EARTH)
-#define RESOLUTION_A (GRAVITY_EARTH / LSG)
-#define CONVERT_A (GRAVITY_EARTH / LSG)
-#define CONVERT_A_X (CONVERT_A)
-#define CONVERT_A_Y (CONVERT_A)
-#define CONVERT_A_Z (CONVERT_A)
-
-/* Compass AKM8975 */
-#define EVENT_TYPE_MAGV_X ABS_RX
-#define EVENT_TYPE_MAGV_Y ABS_RY
-#define EVENT_TYPE_MAGV_Z ABS_RZ
-#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER
-
-// conversion of magnetic data to uT units
-#define CONVERT_M (0.06f)
-
-__END_DECLS
-
-#endif // ANDROID_SENSORS_H
diff --git a/60xx/libsensors_iio/sensors_mpl.cpp b/60xx/libsensors_iio/sensors_mpl.cpp deleted file mode 100644 index 4aef514..0000000 --- a/60xx/libsensors_iio/sensors_mpl.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/*
-* Copyright (C) 2012 Invensense, Inc.
-*
-* Licensed under the Apache License, Version 2.0 (the "License");
-* you may not use this file except in compliance with the License.
-* You may obtain a copy of the License at
-*
-* http://www.apache.org/licenses/LICENSE-2.0
-*
-* Unless required by applicable law or agreed to in writing, software
-* distributed under the License is distributed on an "AS IS" BASIS,
-* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-* See the License for the specific language governing permissions and
-* limitations under the License.
-*/
-
-#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__)
-
-#include <hardware/sensors.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <dirent.h>
-#include <math.h>
-#include <poll.h>
-#include <pthread.h>
-#include <stdlib.h>
-
-#include <linux/input.h>
-
-#include <utils/Atomic.h>
-#include <utils/Log.h>
-
-#include "sensors.h"
-#include "MPLSensor.h"
-
-/*****************************************************************************/
-/* The SENSORS Module */
-
-#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
-#define LOCAL_SENSORS (MPLSensor::numSensors + 1)
-#else
-#define LOCAL_SENSORS MPLSensor::numSensors
-#endif
-
-/* Vendor-defined Accel Load Calibration File Method
-* @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame
-* @return '0' for a successful load, '1' otherwise
-* example: int AccelLoadConfig(long* offset);
-* End of Vendor-defined Accel Load Cal Method
-*/
-
-static struct sensor_t sSensorList[LOCAL_SENSORS];
-static int sensors = (sizeof(sSensorList) / sizeof(sensor_t));
-
-static int open_sensors(const struct hw_module_t* module, const char* id,
- struct hw_device_t** device);
-
-static int sensors__get_sensors_list(struct sensors_module_t* module,
- struct sensor_t const** list)
-{
- *list = sSensorList;
- return sensors;
-}
-
-static struct hw_module_methods_t sensors_module_methods = {
- open: open_sensors
-};
-
-struct sensors_module_t HAL_MODULE_INFO_SYM = {
- common: {
- tag: HARDWARE_MODULE_TAG,
- version_major: 1,
- version_minor: 0,
- id: SENSORS_HARDWARE_MODULE_ID,
- name: "Invensense module",
- author: "Invensense Inc.",
- methods: &sensors_module_methods,
- },
- get_sensors_list: sensors__get_sensors_list,
-};
-
-struct sensors_poll_context_t {
- struct sensors_poll_device_t device; // must be first
-
- sensors_poll_context_t();
- ~sensors_poll_context_t();
- int activate(int handle, int enabled);
- int setDelay(int handle, int64_t ns);
- int pollEvents(sensors_event_t* data, int count);
-
-private:
- enum {
- mpl = 0,
- compass,
- dmpOrient,
- numSensorDrivers, // wake pipe goes here
- numFds,
- };
-
- struct pollfd mPollFds[numSensorDrivers];
- SensorBase *mSensor;
-};
-
-/******************************************************************************/
-
-sensors_poll_context_t::sensors_poll_context_t() {
- VFUNC_LOG;
-
- mCompassSensor = new CompassSensor();
- MPLSensor *mplSensor = new MPLSensor(mCompassSensor);
-
- /* For Vendor-defined Accel Calibration File Load
- * Use the Following Constructor and Pass Your Load Cal File Function
- *
- * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
- */
-
- // setup the callback object for handing mpl callbacks
- setCallbackObject(mplSensor);
-
- // populate the sensor list
- sensors =
- mplSensor->populateSensorList(sSensorList, sizeof(sSensorList));
-
- mSensor = mplSensor;
- mPollFds[mpl].fd = mSensor->getFd();
- mPollFds[mpl].events = POLLIN;
- mPollFds[mpl].revents = 0;
-
- mPollFds[compass].fd = mCompassSensor->getFd();
- mPollFds[compass].events = POLLIN;
- mPollFds[compass].revents = 0;
-
- mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd();
- mPollFds[dmpOrient].events = POLLPRI;
- mPollFds[dmpOrient].revents = 0;
-}
-
-sensors_poll_context_t::~sensors_poll_context_t() {
- FUNC_LOG;
- delete mSensor;
-}
-
-int sensors_poll_context_t::activate(int handle, int enabled) {
- FUNC_LOG;
- return mSensor->enable(handle, enabled);
-}
-
-int sensors_poll_context_t::setDelay(int handle, int64_t ns)
-{
- FUNC_LOG;
- return mSensor->setDelay(handle, ns);
-}
-
-int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
-{
- VHANDLER_LOG;
-
- int nbEvents = 0;
- int nb, polltime = -1;
-
- // look for new events
- nb = poll(mPollFds, numSensorDrivers, polltime);
-
- if (nb > 0) {
- for (int i = 0; count && i < numSensorDrivers; i++) {
- if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
- nb = 0;
- if (i == mpl) {
- /* Ignore res */
- mSensor->readEvents(NULL, 0);
- mPollFds[i].revents = 0;
- }
- else if (i == compass) {
- /* Ignore res */
- ((MPLSensor*) mSensor)->readCompassEvents(NULL, count);
- mPollFds[i].revents = 0;
- }
- }
- }
- nb = ((MPLSensor*) mSensor)->executeOnData(data, count);
- if (nb > 0) {
- count -= nb;
- nbEvents += nb;
- data += nb;
- }
-
- if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) {
- nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count);
- mPollFds[dmpOrient].revents= 0;
- if (isDmpScreenAutoRotationEnabled() && nb > 0) {
- count -= nb;
- nbEvents += nb;
- data += nb;
- }
- }
- }
-
- return nbEvents;
-}
-
-/******************************************************************************/
-
-static int poll__close(struct hw_device_t *dev)
-{
- FUNC_LOG;
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- if (ctx) {
- delete ctx;
- }
- return 0;
-}
-
-static int poll__activate(struct sensors_poll_device_t *dev,
- int handle, int enabled)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- return ctx->activate(handle, enabled);
-}
-
-static int poll__setDelay(struct sensors_poll_device_t *dev,
- int handle, int64_t ns)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- int s= ctx->setDelay(handle, ns);
- return s;
-}
-
-static int poll__poll(struct sensors_poll_device_t *dev,
- sensors_event_t* data, int count)
-{
- sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev;
- return ctx->pollEvents(data, count);
-}
-
-/******************************************************************************/
-
-/** Open a new instance of a sensor device using name */
-static int open_sensors(const struct hw_module_t* module, const char* id,
- struct hw_device_t** device)
-{
- FUNC_LOG;
- int status = -EINVAL;
- sensors_poll_context_t *dev = new sensors_poll_context_t();
-
- memset(&dev->device, 0, sizeof(sensors_poll_device_t));
-
- dev->device.common.tag = HARDWARE_DEVICE_TAG;
- dev->device.common.version = 0;
- dev->device.common.module = const_cast<hw_module_t*>(module);
- dev->device.common.close = poll__close;
- dev->device.activate = poll__activate;
- dev->device.setDelay = poll__setDelay;
- dev->device.poll = poll__poll;
-
- *device = &dev->device.common;
- status = 0;
-
- return status;
-}
diff --git a/60xx/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk deleted file mode 100644 index 84e7e9b..0000000 --- a/60xx/libsensors_iio/software/build/android/common.mk +++ /dev/null @@ -1,87 +0,0 @@ -# Use bash for additional echo fancyness -SHELL = /bin/bash - -#################################################################################################### -## defines - -# Build for Jellybean -BUILD_ANDROID_JELLYBEAN = 1 - -## libraries ## -LIB_PREFIX = lib - -STATIC_LIB_EXT = a -SHARED_LIB_EXT = so - -# normally, overridden from outside -# ?= assignment sets it only if not already defined -TARGET ?= android - -MLLITE_LIB_NAME ?= mllite -MPL_LIB_NAME ?= mplmpu - -## applications ## -SHARED_APP_SUFFIX = -shared -STATIC_APP_SUFFIX = -static - -#################################################################################################### -## compile, includes, and linker - -ifeq ($(BUILD_ANDROID_JELLYBEAN),1) -ANDROID_COMPILE = -DANDROID_JELLYBEAN=1 -endif - -ANDROID_LINK = -nostdlib -ANDROID_LINK += -fpic -ANDROID_LINK += -Wl,--gc-sections -ANDROID_LINK += -Wl,--no-whole-archive -ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib -ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib - -ANDROID_LINK_EXECUTABLE = $(ANDROID_LINK) -ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker -ifneq ($(BUILD_ANDROID_JELLYBEAN),1) -ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x -endif -ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o -ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o - -ANDROID_INCLUDES = -I$(ANDROID_ROOT)/system/core/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include # ICS -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm -ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates - -KERNEL_INCLUDES = -I$(KERNEL_ROOT)/include - -INV_INCLUDES = -I$(INV_ROOT)/software/core/driver/include -INV_INCLUDES += -I$(MLLITE_DIR) -INV_INCLUDES += -I$(MLLITE_DIR)/linux - -INV_DEFINES += -DINV_CACHE_DMP=1 - -#################################################################################################### -## macros - -ifndef echo_in_colors -define echo_in_colors - echo -ne "\e[1;32m"$(1)"\e[0m" -endef -endif - - - diff --git a/60xx/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk deleted file mode 100644 index 47dedfe..0000000 --- a/60xx/libsensors_iio/software/build/android/shared.mk +++ /dev/null @@ -1,74 +0,0 @@ -SHELL=/bin/bash - -TARGET ?= android -PRODUCT ?= beagleboard -ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair -KERNEL_ROOT ?= /Android/trunk/0xdroid/kernel -MLSDK_ROOT ?= $(CURDIR) - -ifeq ($(VERBOSE),1) - DUMP=1>/dev/stdout -else - DUMP=1>/dev/null -endif - -include common.mk - -################################################################################ -## targets - -INV_ROOT = ../.. -LIB_FOLDERS = $(INV_ROOT)/core/mllite/build/$(TARGET) -ifeq ($(BUILD_MPL),1) - LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET) -endif -APP_FOLDERS = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET) -APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET) -APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET) - -INSTALL_DIR = $(CURDIR) - -################################################################################ -## macros - -define echo_in_colors - echo -ne "\e[1;34m"$(1)"\e[0m" -endef - -################################################################################ -## rules - -.PHONY : all mllite mpl clean - -all: - for DIR in $(LIB_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - for DIR in $(APP_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - -clean: - for DIR in $(LIB_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - for DIR in $(APP_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - -cleanall: - for DIR in $(LIB_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - for DIR in $(APP_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - -install: - for DIR in $(LIB_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - for DIR in $(APP_FOLDERS); do ( \ - cd $$DIR && $(MAKE) -f shared.mk $@ ); \ - done - diff --git a/60xx/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h deleted file mode 100644 index c519691..0000000 --- a/60xx/libsensors_iio/software/core/driver/include/log.h +++ /dev/null @@ -1,364 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/* - * This file incorporates work covered by the following copyright and - * permission notice: - * - * Copyright (C) 2005 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* - * C/C++ logging functions. See the logging documentation for API details. - * - * We'd like these to be available from C code (in case we import some from - * somewhere), so this has a C interface. - * - * The output will be correct when the log file is shared between multiple - * threads and/or multiple processes so long as the operating system - * supports O_APPEND. These calls have mutex-protected data structures - * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. - */ -#ifndef _LIBS_CUTILS_MPL_LOG_H -#define _LIBS_CUTILS_MPL_LOG_H - -#include <stdlib.h> -#include <stdarg.h> - -#ifdef ANDROID -#ifdef NDK_BUILD -#include "log_macros.h" -#else -#include <utils/Log.h> /* For the LOG macro */ -#endif -#endif - -#ifdef __KERNEL__ -#include <linux/kernel.h> -#endif - -#ifdef __cplusplus -extern "C" { -#endif - - -/* --------------------------------------------------------------------- */ - -/* - * Normally we strip MPL_LOGV (VERBOSE messages) from release builds. - * You can modify this (for example with "#define MPL_LOG_NDEBUG 0" - * at the top of your source file) to change that behavior. - */ -#ifndef MPL_LOG_NDEBUG -#ifdef NDEBUG -#define MPL_LOG_NDEBUG 1 -#else -#define MPL_LOG_NDEBUG 0 -#endif -#endif - -#ifdef __KERNEL__ -#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE -#define MPL_LOG_DEFAULT KERN_DEFAULT -#define MPL_LOG_VERBOSE KERN_CONT -#define MPL_LOG_DEBUG KERN_NOTICE -#define MPL_LOG_INFO KERN_INFO -#define MPL_LOG_WARN KERN_WARNING -#define MPL_LOG_ERROR KERN_ERR -#define MPL_LOG_SILENT MPL_LOG_VERBOSE - -#else - /* Based off the log priorities in android - /system/core/include/android/log.h */ -#define MPL_LOG_UNKNOWN (0) -#define MPL_LOG_DEFAULT (1) -#define MPL_LOG_VERBOSE (2) -#define MPL_LOG_DEBUG (3) -#define MPL_LOG_INFO (4) -#define MPL_LOG_WARN (5) -#define MPL_LOG_ERROR (6) -#define MPL_LOG_SILENT (8) -#endif - - -/* - * This is the local tag used for the following simplified - * logging macros. You can change this preprocessor definition - * before using the other macros to change the tag. - */ -#ifndef MPL_LOG_TAG -#ifdef __KERNEL__ -#define MPL_LOG_TAG -#else -#define MPL_LOG_TAG NULL -#endif -#endif - -/* --------------------------------------------------------------------- */ - -/* - * Simplified macro to send a verbose log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGV -#if MPL_LOG_NDEBUG -#ifdef _WIN32 -#define MPL_LOGV(fmt, ...) \ - do { \ - __pragma (warning(suppress : 4127 )) \ - if (0) \ - MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ - __pragma (warning(suppress : 4127 )) \ - } while (0) -#else -#define MPL_LOGV(fmt, ...) \ - do { \ - if (0) \ - MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ - } while (0) -#endif - -#else -#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef CONDITION -#define CONDITION(cond) ((cond) != 0) -#endif - -#ifndef MPL_LOGV_IF -#if MPL_LOG_NDEBUG -#define MPL_LOGV_IF(cond, fmt, ...) \ - do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0) -#else -#define MPL_LOGV_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif -#endif - -/* - * Simplified macro to send a debug log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGD -#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif - -#ifndef MPL_LOGD_IF -#define MPL_LOGD_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* - * Simplified macro to send an info log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGI -#ifdef __KERNEL__ -#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__) -#else -#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef MPL_LOGI_IF -#define MPL_LOGI_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* - * Simplified macro to send a warning log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGW -#ifdef __KERNEL__ -#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__) -#else -#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef MPL_LOGW_IF -#define MPL_LOGW_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* - * Simplified macro to send an error log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGE -#ifdef __KERNEL__ -#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__) -#else -#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef MPL_LOGE_IF -#define MPL_LOGE_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* --------------------------------------------------------------------- */ - -/* - * Log a fatal error. If the given condition fails, this stops program - * execution like a normal assertion, but also generating the given message. - * It is NOT stripped from release builds. Note that the condition test - * is -inverted- from the normal assert() semantics. - */ -#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \ - fmt, ##__VA_ARGS__)) \ - : (void)0) - -#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \ - (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__))) - -/* - * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that - * are stripped out of release builds. - */ -#if MPL_LOG_NDEBUG -#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ - do { \ - if (0) \ - MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \ - } while (0) -#define MPL_LOG_FATAL(fmt, ...) \ - do { \ - if (0) \ - MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \ - } while (0) -#else -#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ - MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__) -#define MPL_LOG_FATAL(fmt, ...) \ - MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) -#endif - -/* - * Assertion that generates a log message when the assertion fails. - * Stripped out of release builds. Uses the current MPL_LOG_TAG. - */ -#define MPL_LOG_ASSERT(cond, fmt, ...) \ - MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__) - -/* --------------------------------------------------------------------- */ - -/* - * Basic log message macro. - * - * Example: - * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno); - * - * The second argument may be NULL or "" to indicate the "global" tag. - */ -#ifndef MPL_LOG -#define MPL_LOG(priority, tag, fmt, ...) \ - MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__) -#endif - -/* - * Log macro that allows you to specify a number for the priority. - */ -#ifndef MPL_LOG_PRI -#ifdef ANDROID -#define MPL_LOG_PRI(priority, tag, fmt, ...) \ - ALOG(priority, tag, fmt, ##__VA_ARGS__) -#elif defined __KERNEL__ -#define MPL_LOG_PRI(priority, tag, fmt, ...) \ - pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__) -#else -#define MPL_LOG_PRI(priority, tag, fmt, ...) \ - _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__) -#endif -#endif - -/* - * Log macro that allows you to pass in a varargs ("args" is a va_list). - */ -#ifndef MPL_LOG_PRI_VA -#ifdef ANDROID -#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ - android_vprintLog(priority, NULL, tag, fmt, args) -#elif defined __KERNEL__ -/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */ -#else -#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ - _MLPrintVaLog(priority, NULL, tag, fmt, args) -#endif -#endif - -/* --------------------------------------------------------------------- */ - -/* - * =========================================================================== - * - * The stuff in the rest of this file should not be used directly. - */ - -#ifndef ANDROID -int _MLPrintLog(int priority, const char *tag, const char *fmt, ...); -int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args); -/* Final implementation of actual writing to a character device */ -int _MLWriteLog(const char *buf, int buflen); -#endif - -static inline void __print_result_location(int result, - const char *file, - const char *func, int line) -{ - MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result); -} - -#ifdef _WIN32 -/* The pragma removes warning about expression being constant */ -#define LOG_RESULT_LOCATION(condition) \ - do { \ - __print_result_location((int)(condition), __FILE__, \ - __func__, __LINE__); \ - __pragma (warning(suppress : 4127 )) \ - } while (0) -#else -#define LOG_RESULT_LOCATION(condition) \ - do { \ - __print_result_location((int)(condition), __FILE__, \ - __func__, __LINE__); \ - } while (0) -#endif - - -#define INV_ERROR_CHECK(r_1329) \ - if (r_1329) { \ - LOG_RESULT_LOCATION(r_1329); \ - return r_1329; \ - } - -#ifdef __cplusplus -} -#endif -#endif /* _LIBS_CUTILS_MPL_LOG_H */ diff --git a/60xx/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h deleted file mode 100644 index 9725199..0000000 --- a/60xx/libsensors_iio/software/core/driver/include/mlinclude.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef INV_INCLUDE_H__ -#define INV_INCLUDE_H__ - -#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere - -#ifdef COVERAGE -#include "utestCommon.h" -#endif -#ifdef PROFILE -#include "profile.h" -#endif - -#ifdef WIN32 -#ifdef COVERAGE - -extern int functionEnterLog(const char *file, const char *func); -extern int functionExitLog(const char *file, const char *func); - -#undef INVENSENSE_FUNC_START -#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \ - int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__) -#endif // COVERAGE -#endif // WIN32 - -#ifdef PROFILE -#undef INVENSENSE_FUNC_START -#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__) -#define return if ( profileExit(__FILE__, __FUNCTION__) ) return -#endif // PROFILE - -// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return - -#endif //INV_INCLUDE_H__ diff --git a/60xx/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h deleted file mode 100644 index 37194d6..0000000 --- a/60xx/libsensors_iio/software/core/driver/include/mlmath.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/******************************************************************************* - * - * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#ifndef _ML_MATH_H_ -#define _ML_MATH_H_ - -#ifndef MLMATH -// This define makes Microsoft pickup things like M_PI -#define _USE_MATH_DEFINES -#include <math.h> - -#ifdef WIN32 -// Microsoft doesn't follow standards -#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5)))) -#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) -#endif - -#else // MLMATH - -#ifdef __cplusplus -extern "C" { -#endif -/* MPL needs below functions */ -double ml_asin(double); -double ml_atan(double); -double ml_atan2(double, double); -double ml_log(double); -double ml_sqrt(double); -double ml_ceil(double); -double ml_floor(double); -double ml_cos(double); -double ml_sin(double); -double ml_acos(double); -#ifdef __cplusplus -} // extern "C" -#endif - -/* - * We rename functions here to provide the hook for other - * customized math functions. - */ -#define sqrt(x) ml_sqrt(x) -#define log(x) ml_log(x) -#define asin(x) ml_asin(x) -#define atan(x) ml_atan(x) -#define atan2(x,y) ml_atan2(x,y) -#define ceil(x) ml_ceil(x) -#define floor(x) ml_floor(x) -#define fabs(x) (((x)<0)?-(x):(x)) -#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5)))) -#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) -#define cos(x) ml_cos(x) -#define sin(x) ml_sin(x) -#define acos(x) ml_acos(x) - -#define pow(x,y) ml_pow(x,y) - -#ifdef LINUX -/* stubs for float version of math functions */ -#define cosf(x) ml_cos(x) -#define sinf(x) ml_sin(x) -#define atan2f(x,y) ml_atan2(x,y) -#define sqrtf(x) ml_sqrt(x) -#endif - - - -#endif // MLMATH - -#ifndef M_PI -#define M_PI 3.14159265358979 -#endif - -#ifndef ABS -#define ABS(x) (((x)>=0)?(x):-(x)) -#endif - -#ifndef MIN -#define MIN(x,y) (((x)<(y))?(x):(y)) -#endif - -#ifndef MAX -#define MAX(x,y) (((x)>(y))?(x):(y)) -#endif - -/*---------------------------*/ -#endif /* !_ML_MATH_H_ */ diff --git a/60xx/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h deleted file mode 100644 index 12f2901..0000000 --- a/60xx/libsensors_iio/software/core/driver/include/mlsl.h +++ /dev/null @@ -1,283 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef __MLSL_H__ -#define __MLSL_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * @defgroup MLSL - * @brief Motion Library - Serial Layer. - * The Motion Library System Layer provides the Motion Library - * with the communication interface to the hardware. - * - * The communication interface is assumed to support serial - * transfers in burst of variable length up to - * SERIAL_MAX_TRANSFER_SIZE. - * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes. - * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will - * be subdivided in smaller transfers of length <= - * SERIAL_MAX_TRANSFER_SIZE. - * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to - * overcome any host processor transfer size limitation down to - * 1 B, the minimum. - * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor - * performance and efficiency while requiring higher resource usage - * (mostly buffering). A smaller value will increase overhead and - * decrease efficiency but allows to operate with more resource - * constrained processor and master serial controllers. - * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the - * mlsl.h header file and master serial controllers. - * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the - * mlsl.h header file. - * - * @{ - * @file mlsl.h - * @brief The Motion Library System Layer. - * - */ - -/* - * NOTE : to properly support Yamaha compass reads, - * the max transfer size should be at least 9 B. - * Length in bytes, typically a power of 2 >= 2 - */ -#define SERIAL_MAX_TRANSFER_SIZE 31 - -#ifndef __KERNEL__ -/** - * inv_serial_open() - used to open the serial port. - * @port The COM port specification associated with the device in use. - * @sl_handle a pointer to the file handle to the serial device to be open - * for the communication. - * This port is used to send and receive data to the device. - * - * This function is called by inv_serial_start(). - * Unlike previous MPL Software releases, explicitly calling - * inv_serial_start() is mandatory to instantiate the communication - * with the device. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_open(char const *port, void **sl_handle); - -/** - * inv_serial_close() - used to close the serial port. - * @sl_handle a file handle to the serial device used for the communication. - * - * This port is used to send and receive data to the device. - * - * This function is called by inv_serial_stop(). - * Unlike previous MPL Software releases, explicitly calling - * inv_serial_stop() is mandatory to properly shut-down the - * communication with the device. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_close(void *sl_handle); - -/** - * inv_serial_reset() - used to reset any buffering the driver may be doing - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_reset(void *sl_handle); -#endif - -/** - * inv_serial_single_write() - used to write a single byte of data. - * @sl_handle pointer to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @register_addr Register address to write. - * @data Single byte of data to write. - * - * It is called by the MPL to write a single byte of data to the MPU. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_single_write( - void *sl_handle, - unsigned char slave_addr, - unsigned char register_addr, - unsigned char data); - -/** - * inv_serial_write() - used to write multiple bytes of data to registers. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @register_addr Register address to write. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_write( - void *sl_handle, - unsigned char slave_addr, - unsigned short length, - unsigned char const *data); - -/** - * inv_serial_read() - used to read multiple bytes of data from registers. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @register_addr Register address to read. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -int inv_serial_read( - void *sl_handle, - unsigned char slave_addr, - unsigned char register_addr, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_read_mem() - used to read multiple bytes of data from the memory. - * This should be sent by I2C or SPI. - * - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @mem_addr The location in the memory to read from. - * @length Length of burst data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -int inv_serial_read_mem( - void *sl_handle, - unsigned char slave_addr, - unsigned short mem_addr, - unsigned char bank_reg, - unsigned char addr_reg, - unsigned char mem_reg, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_write_mem() - used to write multiple bytes of data to the memory. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @mem_addr The location in the memory to write to. - * @length Length of burst data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -int inv_serial_write_mem( - void *sl_handle, - unsigned char slave_addr, - unsigned short mem_addr, - unsigned char bank_reg, - unsigned char addr_reg, - unsigned char mem_reg, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -int inv_serial_read_fifo( - void *sl_handle, - unsigned char slave_addr, - unsigned char fifo_reg, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -int inv_serial_write_fifo( - void *sl_handle, - unsigned char slave_addr, - unsigned char fifo_reg, - unsigned short length, - unsigned char const *data); - -#ifndef __KERNEL__ -/** - * inv_serial_read_cfg() - used to get the configuration data. - * @cfg Pointer to the configuration data. - * @len Length of the configuration data. - * - * Is called by the MPL to get the configuration data - * used by the motion library. - * This data would typically be saved in non-volatile memory. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_read_cfg(unsigned char *cfg, unsigned int len); - -/** - * inv_serial_write_cfg() - used to save the configuration data. - * @cfg Pointer to the configuration data. - * @len Length of the configuration data. - * - * Is called by the MPL to save the configuration data used by the - * motion library. - * This data would typically be saved in non-volatile memory. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_write_cfg(unsigned char *cfg, unsigned int len); - -/** - * inv_serial_read_cal() - used to get the calibration data. - * @cfg Pointer to the calibration data. - * @len Length of the calibration data. - * - * It is called by the MPL to get the calibration data used by the - * motion library. - * This data is typically be saved in non-volatile memory. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_read_cal(unsigned char *cal, unsigned int len); - -/** - * inv_serial_write_cal() - used to save the calibration data. - * - * @cfg Pointer to the calibration data. - * @len Length of the calibration data. - * - * It is called by the MPL to save the calibration data used by the - * motion library. - * This data is typically be saved in non-volatile memory. - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_write_cal(unsigned char *cal, unsigned int len); - -/** - * inv_serial_get_cal_length() - Get the calibration length from the storage. - * @len lenght to be returned - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -int inv_serial_get_cal_length(unsigned int *len); -#endif -#ifdef __cplusplus -} -#endif -/** - * @} - */ -#endif /* __MLSL_H__ */ diff --git a/60xx/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h deleted file mode 100644 index 09eccce..0000000 --- a/60xx/libsensors_iio/software/core/driver/include/mltypes.h +++ /dev/null @@ -1,235 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/** - * @defgroup MLERROR - * @brief Motion Library - Error definitions. - * Definition of the error codes used within the MPL and - * returned to the user. - * Every function tries to return a meaningful error code basing - * on the occuring error condition. The error code is numeric. - * - * The available error codes and their associated values are: - * - (0) INV_SUCCESS - * - (32) INV_ERROR - * - (22 / EINVAL) INV_ERROR_INVALID_PARAMETER - * - (1 / EPERM) INV_ERROR_FEATURE_NOT_ENABLED - * - (36) INV_ERROR_FEATURE_NOT_IMPLEMENTED - * - (38) INV_ERROR_DMP_NOT_STARTED - * - (39) INV_ERROR_DMP_STARTED - * - (40) INV_ERROR_NOT_OPENED - * - (41) INV_ERROR_OPENED - * - (19 / ENODEV) INV_ERROR_INVALID_MODULE - * - (12 / ENOMEM) INV_ERROR_MEMORY_EXAUSTED - * - (44) INV_ERROR_DIVIDE_BY_ZERO - * - (45) INV_ERROR_ASSERTION_FAILURE - * - (46) INV_ERROR_FILE_OPEN - * - (47) INV_ERROR_FILE_READ - * - (48) INV_ERROR_FILE_WRITE - * - (49) INV_ERROR_INVALID_CONFIGURATION - * - (52) INV_ERROR_SERIAL_CLOSED - * - (53) INV_ERROR_SERIAL_OPEN_ERROR - * - (54) INV_ERROR_SERIAL_READ - * - (55) INV_ERROR_SERIAL_WRITE - * - (56) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED - * - (57) INV_ERROR_SM_TRANSITION - * - (58) INV_ERROR_SM_IMPROPER_STATE - * - (62) INV_ERROR_FIFO_OVERFLOW - * - (63) INV_ERROR_FIFO_FOOTER - * - (64) INV_ERROR_FIFO_READ_COUNT - * - (65) INV_ERROR_FIFO_READ_DATA - * - (72) INV_ERROR_MEMORY_SET - * - (82) INV_ERROR_LOG_MEMORY_ERROR - * - (83) INV_ERROR_LOG_OUTPUT_ERROR - * - (92) INV_ERROR_OS_BAD_PTR - * - (93) INV_ERROR_OS_BAD_HANDLE - * - (94) INV_ERROR_OS_CREATE_FAILED - * - (95) INV_ERROR_OS_LOCK_FAILED - * - (102) INV_ERROR_COMPASS_DATA_OVERFLOW - * - (103) INV_ERROR_COMPASS_DATA_UNDERFLOW - * - (104) INV_ERROR_COMPASS_DATA_NOT_READY - * - (105) INV_ERROR_COMPASS_DATA_ERROR - * - (107) INV_ERROR_CALIBRATION_LOAD - * - (108) INV_ERROR_CALIBRATION_STORE - * - (109) INV_ERROR_CALIBRATION_LEN - * - (110) INV_ERROR_CALIBRATION_CHECKSUM - * - (111) INV_ERROR_ACCEL_DATA_OVERFLOW - * - (112) INV_ERROR_ACCEL_DATA_UNDERFLOW - * - (113) INV_ERROR_ACCEL_DATA_NOT_READY - * - (114) INV_ERROR_ACCEL_DATA_ERROR - * - * The available warning codes and their associated values are: - * - (115) INV_WARNING_MOTION_RACE - * - (116) INV_WARNING_QUAT_TRASHED - * - * @{ - * @file mltypes.h - * @} - */ - -#ifndef MLTYPES_H -#define MLTYPES_H - -#ifdef __KERNEL__ -#include <linux/types.h> -#include <asm-generic/errno-base.h> -#else -#include "stdint_invensense.h" -#include <errno.h> -#endif -#include <limits.h> - -#ifndef REMOVE_INV_ERROR_T -/*--------------------------- - * ML Types - *--------------------------*/ - -/** - * @struct inv_error_t mltypes.h "mltypes" - * @brief The MPL Error Code return type. - * - * @code - * typedef unsigned char inv_error_t; - * @endcode - */ -//typedef unsigned char inv_error_t; -typedef int inv_error_t; -#endif - -typedef long long inv_time_t; - -#if !defined __GNUC__ && !defined __KERNEL__ -typedef int8_t __s8; -typedef int16_t __s16; -typedef int32_t __s32; -typedef int32_t __s64; - -typedef uint8_t __u8; -typedef uint16_t __u16; -typedef uint32_t __u32; -typedef uint64_t __u64; -#elif !defined __KERNEL__ -#include <sys/types.h> -#endif - -#ifndef __cplusplus -#ifndef __KERNEL__ -typedef int_fast8_t bool; - -#ifndef false -#define false 0 -#endif - -#ifndef true -#define true 1 -#endif - -#endif -#endif - -/*--------------------------- - * ML Defines - *--------------------------*/ - -#ifndef NULL -#define NULL 0 -#endif - -#ifndef __KERNEL__ -#ifndef ARRAY_SIZE -/* Dimension of an array */ -#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0])) -#endif -#endif -/* - ML Errors. - */ -#define ERROR_NAME(x) (#x) -#define ERROR_CHECK_FIRST(first, x) \ - { if (INV_SUCCESS == first) first = x; } - -#define INV_SUCCESS (0) -/* Generic Error code. Proprietary Error Codes only */ -#define INV_ERROR_BASE (0x20) -#define INV_ERROR (INV_ERROR_BASE) - -/* Compatibility and other generic error codes */ -#define INV_ERROR_INVALID_PARAMETER (EINVAL) -#define INV_ERROR_FEATURE_NOT_ENABLED (EPERM) -#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (INV_ERROR_BASE + 4) -#define INV_ERROR_DMP_NOT_STARTED (INV_ERROR_BASE + 6) -#define INV_ERROR_DMP_STARTED (INV_ERROR_BASE + 7) -#define INV_ERROR_NOT_OPENED (INV_ERROR_BASE + 8) -#define INV_ERROR_OPENED (INV_ERROR_BASE + 9) -#define INV_ERROR_INVALID_MODULE (ENODEV) -#define INV_ERROR_MEMORY_EXAUSTED (ENOMEM) -#define INV_ERROR_DIVIDE_BY_ZERO (INV_ERROR_BASE + 12) -#define INV_ERROR_ASSERTION_FAILURE (INV_ERROR_BASE + 13) -#define INV_ERROR_FILE_OPEN (INV_ERROR_BASE + 14) -#define INV_ERROR_FILE_READ (INV_ERROR_BASE + 15) -#define INV_ERROR_FILE_WRITE (INV_ERROR_BASE + 16) -#define INV_ERROR_INVALID_CONFIGURATION (INV_ERROR_BASE + 17) -#define INV_ERROR_NOT_AUTHORIZED (INV_ERROR_BASE + 18) - -/* Serial Communication */ -#define INV_ERROR_SERIAL_CLOSED (INV_ERROR_BASE + 20) -#define INV_ERROR_SERIAL_OPEN_ERROR (INV_ERROR_BASE + 21) -#define INV_ERROR_SERIAL_READ (INV_ERROR_BASE + 22) -#define INV_ERROR_SERIAL_WRITE (INV_ERROR_BASE + 23) -#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (INV_ERROR_BASE + 24) - -/* SM = State Machine */ -#define INV_ERROR_SM_TRANSITION (INV_ERROR_BASE + 25) -#define INV_ERROR_SM_IMPROPER_STATE (INV_ERROR_BASE + 26) - -/* Fifo */ -#define INV_ERROR_FIFO_OVERFLOW (INV_ERROR_BASE + 30) -#define INV_ERROR_FIFO_FOOTER (INV_ERROR_BASE + 31) -#define INV_ERROR_FIFO_READ_COUNT (INV_ERROR_BASE + 32) -#define INV_ERROR_FIFO_READ_DATA (INV_ERROR_BASE + 33) - -/* Memory & Registers, Set & Get */ -#define INV_ERROR_MEMORY_SET (INV_ERROR_BASE + 40) - -#define INV_ERROR_LOG_MEMORY_ERROR (INV_ERROR_BASE + 50) -#define INV_ERROR_LOG_OUTPUT_ERROR (INV_ERROR_BASE + 51) - -/* OS interface errors */ -#define INV_ERROR_OS_BAD_PTR (INV_ERROR_BASE + 60) -#define INV_ERROR_OS_BAD_HANDLE (INV_ERROR_BASE + 61) -#define INV_ERROR_OS_CREATE_FAILED (INV_ERROR_BASE + 62) -#define INV_ERROR_OS_LOCK_FAILED (INV_ERROR_BASE + 63) - -/* Compass errors */ -#define INV_ERROR_COMPASS_DATA_OVERFLOW (INV_ERROR_BASE + 70) -#define INV_ERROR_COMPASS_DATA_UNDERFLOW (INV_ERROR_BASE + 71) -#define INV_ERROR_COMPASS_DATA_NOT_READY (INV_ERROR_BASE + 72) -#define INV_ERROR_COMPASS_DATA_ERROR (INV_ERROR_BASE + 73) - -/* Load/Store calibration */ -#define INV_ERROR_CALIBRATION_LOAD (INV_ERROR_BASE + 75) -#define INV_ERROR_CALIBRATION_STORE (INV_ERROR_BASE + 76) -#define INV_ERROR_CALIBRATION_LEN (INV_ERROR_BASE + 77) -#define INV_ERROR_CALIBRATION_CHECKSUM (INV_ERROR_BASE + 78) - -/* Accel errors */ -#define INV_ERROR_ACCEL_DATA_OVERFLOW (INV_ERROR_BASE + 79) -#define INV_ERROR_ACCEL_DATA_UNDERFLOW (INV_ERROR_BASE + 80) -#define INV_ERROR_ACCEL_DATA_NOT_READY (INV_ERROR_BASE + 81) -#define INV_ERROR_ACCEL_DATA_ERROR (INV_ERROR_BASE + 82) - -/* No Motion Warning States */ -#define INV_WARNING_MOTION_RACE (INV_ERROR_BASE + 83) -#define INV_WARNING_QUAT_TRASHED (INV_ERROR_BASE + 84) -#define INV_WARNING_GYRO_MAG (INV_ERROR_BASE + 85) - -#define INV_WARNING_SEMAPHORE_TIMEOUT (INV_ERROR_BASE + 86) - - -/* For Linux coding compliance */ -#ifndef __KERNEL__ -#define EXPORT_SYMBOL(x) -#endif - -#endif /* MLTYPES_H */ diff --git a/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h deleted file mode 100644 index b8c2511..0000000 --- a/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef STDINT_INVENSENSE_H -#define STDINT_INVENSENSE_H - -#ifndef WIN32 - -#ifdef __KERNEL__ -#include <linux/types.h> -#else -#include <stdint.h> -#endif - -#else - -#include <windows.h> - -typedef signed char int8_t; -typedef short int16_t; -typedef long int32_t; -typedef long long int64_t; - -typedef unsigned char uint8_t; -typedef unsigned short uint16_t; -typedef unsigned long uint32_t; -typedef unsigned long long uint64_t; - -typedef int int_fast8_t; -typedef int int_fast16_t; -typedef long int_fast32_t; - -typedef unsigned int uint_fast8_t; -typedef unsigned int uint_fast16_t; -typedef unsigned long uint_fast32_t; - -#endif - -#endif // STDINT_INVENSENSE_H diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so Binary files differdeleted file mode 100755 index 9bdd5bc..0000000 --- a/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so +++ /dev/null diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk deleted file mode 100644 index 1418450..0000000 --- a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk +++ /dev/null @@ -1,87 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- -COMP ?= $(CROSS)gcc -LINK ?= $(CROSS)gcc - -OBJFOLDER = $(CURDIR)/obj - -INV_ROOT = ../../../../.. -MLLITE_DIR = $(INV_ROOT)/software/core/mllite - -include $(INV_ROOT)/software/build/android/common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) -CFLAGS += -Wall -CFLAGS += -fpic -CFLAGS += -nostdlib -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -CFLAGS += -DLINUX -CFLAGS += -DANDROID -CFLAGS += -mthumb-interwork -CFLAGS += -fno-exceptions -CFLAGS += -ffunction-sections -CFLAGS += -funwind-tables -CFLAGS += -fstack-protector -CFLAGS += -fno-short-enums -CFLAGS += -fmessage-length=0 -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(INV_ROOT)/simple_apps/common -CFLAGS += $(INV_INCLUDES) -CFLAGS += $(INV_DEFINES) - -LLINK = -lc -LLINK += -lm -LLINK += -lutils -LLINK += -lcutils -LLINK += -lgcc -LLINK += -ldl - -LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += -shared -LFLAGS += -Wl,-soname,$(LIBRARY) -LFLAGS += -Wl,-shared,-Bsymbolic -LFLAGS += $(ANDROID_LINK) -LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib - -#################################################################################################### -## sources - -#INV_SOURCES provided by Makefile.filelist -include ../filelist.mk - -INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) -INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all mllite clean cleanall makefiles - -all: mllite - -mllite: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) - @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") - $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) - -$(OBJFOLDER) : - @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") - mkdir obj - -$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") - $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk deleted file mode 100644 index 011120c..0000000 --- a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk +++ /dev/null @@ -1,42 +0,0 @@ -#### filelist.mk for mllite #### - -# headers only -HEADERS := $(MLLITE_DIR)/invensense.h - -# headers for sources -HEADERS += $(MLLITE_DIR)/data_builder.h -HEADERS += $(MLLITE_DIR)/hal_outputs.h -HEADERS += $(MLLITE_DIR)/message_layer.h -HEADERS += $(MLLITE_DIR)/ml_math_func.h -HEADERS += $(MLLITE_DIR)/mpl.h -HEADERS += $(MLLITE_DIR)/results_holder.h -HEADERS += $(MLLITE_DIR)/start_manager.h -HEADERS += $(MLLITE_DIR)/storage_manager.h - -# headers (linux specific) -HEADERS += $(MLLITE_DIR)/linux/mlos.h -HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h -HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h -HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h - -# sources -SOURCES := $(MLLITE_DIR)/data_builder.c -SOURCES += $(MLLITE_DIR)/hal_outputs.c -SOURCES += $(MLLITE_DIR)/message_layer.c -SOURCES += $(MLLITE_DIR)/ml_math_func.c -SOURCES += $(MLLITE_DIR)/mpl.c -SOURCES += $(MLLITE_DIR)/results_holder.c -SOURCES += $(MLLITE_DIR)/start_manager.c -SOURCES += $(MLLITE_DIR)/storage_manager.c - -# sources (linux specific) -SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c -SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c -SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c -SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c - - -INV_SOURCES += $(SOURCES) - -VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux - diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c deleted file mode 100644 index 0aa418d..0000000 --- a/60xx/libsensors_iio/software/core/mllite/data_builder.c +++ /dev/null @@ -1,1228 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/** - * @defgroup Data_Builder data_builder - * @brief Motion Library - Data Builder - * Constructs and Creates the data for MPL - * - * @{ - * @file data_builder.c - * @brief Data Builder. - */ - -#undef MPL_LOG_NDEBUG -#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ - -#include <string.h> - -#include "ml_math_func.h" -#include "data_builder.h" -#include "mlmath.h" -#include "storage_manager.h" -#include "message_layer.h" -#include "results_holder.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL" - -typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); - -struct process_t { - inv_process_cb_func func; - int priority; - int data_required; -}; - -struct inv_db_save_t { - /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ - long compass_bias[3]; - /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ - long gyro_bias[3]; - /** Temperature when *gyro_bias was stored. */ - long gyro_temp; - /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ - long accel_bias[3]; - /** Temperature when accel bias was stored. */ - long accel_temp; - long gyro_temp_slope[3]; - /** Sensor Accuracy */ - int gyro_accuracy; - int accel_accuracy; - int compass_accuracy; -}; - -struct inv_data_builder_t { - int num_cb; - struct process_t process[INV_MAX_DATA_CB]; - struct inv_db_save_t save; - int compass_disturbance; - int mode; -#ifdef INV_PLAYBACK_DBG - int debug_mode; - int last_mode; - FILE *file; -#endif -}; - -void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); -static void inv_set_contiguous(void); - -static struct inv_data_builder_t inv_data_builder; -static struct inv_sensor_cal_t sensors; - -/** Change this key if the data being stored by this file changes */ -#define INV_DB_SAVE_KEY 53395 - -#ifdef INV_PLAYBACK_DBG - -/** Turn on data logging to allow playback of same scenario at a later time. -* @param[in] file File to write to, must be open. -*/ -void inv_turn_on_data_logging(FILE *file) -{ - MPL_LOGV("input data logging started\n"); - inv_data_builder.file = file; - inv_data_builder.debug_mode = RD_RECORD; -} - -/** Turn off data logging to allow playback of same scenario at a later time. -* File passed to inv_turn_on_data_logging() must be closed after calling this. -*/ -void inv_turn_off_data_logging() -{ - MPL_LOGV("input data logging stopped\n"); - inv_data_builder.debug_mode = RD_NO_DEBUG; - inv_data_builder.file = NULL; -} -#endif - -/** This function receives the data that was stored in non-volatile memory between power off */ -static inv_error_t inv_db_load_func(const unsigned char *data) -{ - memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); - // copy in the saved accuracy in the actual sensors accuracy - sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; - sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; - sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; - // TODO - if (sensors.compass.accuracy == 3) { - inv_set_compass_bias_found(1); - } - return INV_SUCCESS; -} - -/** This function returns the data to be stored in non-volatile memory between power off */ -static inv_error_t inv_db_save_func(unsigned char *data) -{ - memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); - return INV_SUCCESS; -} - -/** Initialize the data builder -*/ -inv_error_t inv_init_data_builder(void) -{ - /* TODO: Hardcode temperature scale/offset here. */ - memset(&inv_data_builder, 0, sizeof(inv_data_builder)); - memset(&sensors, 0, sizeof(sensors)); - return inv_register_load_store(inv_db_load_func, inv_db_save_func, - sizeof(inv_data_builder.save), - INV_DB_SAVE_KEY); -} - -/** Gyro sensitivity. -* @return A scale factor to convert device units to degrees per second scaled by 2^16 -* such that degrees_per_second = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum rate * 2^15. -*/ -long inv_get_gyro_sensitivity() -{ - return sensors.gyro.sensitivity; -} - -/** Accel sensitivity. -* @return A scale factor to convert device units to g's scaled by 2^16 -* such that g_s = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum accel value in g's * 2^15. -*/ -long inv_get_accel_sensitivity(void) -{ - return sensors.accel.sensitivity; -} - -/** Compass sensitivity. -* @return A scale factor to convert device units to micro Tesla scaled by 2^16 -* such that uT = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum uT * 2^15. -*/ -long inv_get_compass_sensitivity(void) -{ - return sensors.compass.sensitivity; -} - -/** Sets orientation and sensitivity field for a sensor. -* @param[out] sensor Structure to apply settings to -* @param[in] orientation Orientation description of how part is mounted. -* @param[in] sensitivity A Scale factor to convert from hardware units to -* standard units (dps, uT, g). -*/ -void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, - int orientation, long sensitivity) -{ - sensor->sensitivity = sensitivity; - sensor->orientation = orientation; -} - -/** Sets the Orientation and Sensitivity of the gyro data. -* @param[in] orientation A scalar defining the transformation from chip mounting -* to the body frame. The function inv_orientation_matrix_to_scalar() -* can convert the transformation matrix to this scalar and describes the -* scalar in further detail. -* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 -* such that degrees_per_second = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum rate * 2^15. -*/ -void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_G_ORIENT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); - fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); - } -#endif - set_sensor_orientation_and_scale(&sensors.gyro, orientation, - sensitivity); -} - -/** Set Gyro Sample rate in micro seconds. -* @param[in] sample_rate_us Set Gyro Sample rate in us -*/ -void inv_set_gyro_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.gyro.sample_rate_us = sample_rate_us; - sensors.gyro.sample_rate_ms = sample_rate_us / 1000; - if (sensors.gyro.bandwidth == 0) { - sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); - } -} - -/** Set Accel Sample rate in micro seconds. -* @param[in] sample_rate_us Set Accel Sample rate in us -*/ -void inv_set_accel_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.accel.sample_rate_us = sample_rate_us; - sensors.accel.sample_rate_ms = sample_rate_us / 1000; - if (sensors.accel.bandwidth == 0) { - sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); - } -} - -/** Pick the smallest non-negative number. Priority to td1 on equal -* If both are negative, return the largest. -*/ -static int inv_pick_best_time_difference(long td1, long td2) -{ - if (td1 >= 0) { - if (td2 >= 0) { - if (td1 <= td2) { - // td1 - return 0; - } else { - // td2 - return 1; - } - } else { - // td1 - return 0; - } - } else if (td2 >= 0) { - // td2 - return 1; - } else { - // Both are negative - if (td1 >= td2) { - // td1 - return 0; - } else { - // td2 - return 1; - } - } -} - -/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data. -*/ -static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts) -{ - int status = 0; - switch (sensor_number) { - case 0: // Quat - *ts = sensors.quat.timestamp; - if (inv_data_builder.mode & INV_QUAT_NEW) - if (sensors.quat.timestamp_prev != sensors.quat.timestamp) - status = 1; - return status; - case 1: // Gyro - *ts = sensors.gyro.timestamp; - if (inv_data_builder.mode & INV_GYRO_NEW) - if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp) - status = 1; - return status; - case 2: // Accel - *ts = sensors.accel.timestamp; - if (inv_data_builder.mode & INV_ACCEL_NEW) - if (sensors.accel.timestamp_prev != sensors.accel.timestamp) - status = 1; - return status; - case 3: // Compass - *ts = sensors.compass.timestamp; - if (inv_data_builder.mode & INV_MAG_NEW) - if (sensors.compass.timestamp_prev != sensors.compass.timestamp) - status = 1; - return status; - default: - *ts = 0; - return 0; - } - return 0; -} - -/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. -* It does this by finding a raw sensor that has the closest sample rate that is at least as -* often desired. It also returns if that raw sensor has a new piece of data. -* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel -* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. -*/ -int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts) -{ - long td[2]; - int idx; - - if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { - // Sensor number is 0 (Quat) - return inv_raw_sensor_timestamp(0, ts); - } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { - return 0; // Accel must be on or 6-axis quat must be on - } - - // At this point, we know accel is on. Check if 3-axis quat is on - td[0] = sample_rate_us - sensors.quat.sample_rate_us; - td[1] = sample_rate_us - sensors.accel.sample_rate_us; - if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { - idx = inv_pick_best_time_difference(td[0], td[1]); - idx *= 2; - // 0 = quat, 3=accel - return inv_raw_sensor_timestamp(idx, ts); - } - - // No Quaternion data from outside, Gyro must be on - if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { - return 0; // Gyro must be on - } - - td[0] = sample_rate_us - sensors.gyro.sample_rate_us; - idx = inv_pick_best_time_difference(td[0], td[1]); - idx *= 2; // 0=gyro 2=accel - idx++; - // 1 = gyro, 3=accel - return inv_raw_sensor_timestamp(idx, ts); -} - -/** Set Compass Sample rate in micro seconds. -* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. -*/ -void inv_set_compass_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.compass.sample_rate_us = sample_rate_us; - sensors.compass.sample_rate_ms = sample_rate_us / 1000; - if (sensors.compass.bandwidth == 0) { - sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); - } -} - -void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) -{ - *sample_rate_ms = sensors.gyro.sample_rate_ms; -} - -void inv_get_accel_sample_rate_ms(long *sample_rate_ms) -{ - *sample_rate_ms = sensors.accel.sample_rate_ms; -} - -void inv_get_compass_sample_rate_ms(long *sample_rate_ms) -{ - *sample_rate_ms = sensors.compass.sample_rate_ms; -} - -/** Set Quat Sample rate in micro seconds. -* @param[in] sample_rate_us Set Quat Sample rate in us -*/ -void inv_set_quat_sample_rate(long sample_rate_us) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); - } -#endif - sensors.quat.sample_rate_us = sample_rate_us; - sensors.quat.sample_rate_ms = sample_rate_us / 1000; -} - -/** Set Gyro Bandwidth in Hz -* @param[in] bandwidth_hz Gyro bandwidth in Hz -*/ -void inv_set_gyro_bandwidth(int bandwidth_hz) -{ - sensors.gyro.bandwidth = bandwidth_hz; -} - -/** Set Accel Bandwidth in Hz -* @param[in] bandwidth_hz Gyro bandwidth in Hz -*/ -void inv_set_accel_bandwidth(int bandwidth_hz) -{ - sensors.accel.bandwidth = bandwidth_hz; -} - -/** Set Compass Bandwidth in Hz -* @param[in] bandwidth_hz Gyro bandwidth in Hz -*/ -void inv_set_compass_bandwidth(int bandwidth_hz) -{ - sensors.compass.bandwidth = bandwidth_hz; -} - -/** Helper function stating whether the compass is on or off. - * @return TRUE if compass if on, 0 if compass if off -*/ -int inv_get_compass_on() -{ - return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; -} - -/** Helper function stating whether the gyro is on or off. - * @return TRUE if gyro if on, 0 if gyro if off -*/ -int inv_get_gyro_on() -{ - return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; -} - -/** Helper function stating whether the acceleromter is on or off. - * @return TRUE if accel if on, 0 if accel if off -*/ -int inv_get_accel_on() -{ - return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; -} - -/** Get last timestamp across all 3 sensors that are on. -* This find out which timestamp has the largest value for sensors that are on. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_time_t inv_get_last_timestamp() -{ - inv_time_t timestamp = 0; - if (sensors.accel.status & INV_SENSOR_ON) { - timestamp = sensors.accel.timestamp; - } - if (sensors.gyro.status & INV_SENSOR_ON) { - if (timestamp < sensors.gyro.timestamp) { - timestamp = sensors.gyro.timestamp; - } - } - if (sensors.compass.status & INV_SENSOR_ON) { - if (timestamp < sensors.compass.timestamp) { - timestamp = sensors.compass.timestamp; - } - } - if (sensors.temp.status & INV_SENSOR_ON) { - if (timestamp < sensors.temp.timestamp) - timestamp = sensors.temp.timestamp; - } - return timestamp; -} - -/** Sets the orientation and sensitivity of the gyro data. -* @param[in] orientation A scalar defining the transformation from chip mounting -* to the body frame. The function inv_orientation_matrix_to_scalar() -* can convert the transformation matrix to this scalar and describes the -* scalar in further detail. -* @param[in] sensitivity A scale factor to convert device units to g's -* such that g's = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum g_value * 2^15. -*/ -void inv_set_accel_orientation_and_scale(int orientation, long sensitivity) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_A_ORIENT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); - fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); - } -#endif - set_sensor_orientation_and_scale(&sensors.accel, orientation, - sensitivity); -} - -/** Sets the Orientation and Sensitivity of the gyro data. -* @param[in] orientation A scalar defining the transformation from chip mounting -* to the body frame. The function inv_orientation_matrix_to_scalar() -* can convert the transformation matrix to this scalar and describes the -* scalar in further detail. -* @param[in] sensitivity A scale factor to convert device units to uT -* such that uT = device_units * sensitivity / 2^30. Typically -* it works out to be the maximum uT_value * 2^15. -*/ -void inv_set_compass_orientation_and_scale(int orientation, long sensitivity) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_C_ORIENT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); - fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); - } -#endif - set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); -} - -void inv_matrix_vector_mult(const long *A, const long *x, long *y) -{ - y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); - y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); - y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); -} - -/** Takes raw data stored in the sensor, removes bias, and converts it to -* calibrated data in the body frame. Also store raw data for body frame. -* @param[in,out] sensor structure to modify -* @param[in] bias bias in the mounting frame, in hardware units scaled by -* 2^16. Length 3. -*/ -void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) -{ - long raw32[3]; - - // Convert raw to calibrated - raw32[0] = (long)sensor->raw[0] << 15; - raw32[1] = (long)sensor->raw[1] << 15; - raw32[2] = (long)sensor->raw[2] << 15; - - inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); - - raw32[0] -= bias[0] >> 1; - raw32[1] -= bias[1] >> 1; - raw32[2] -= bias[2] >> 1; - - inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); - - sensor->status |= INV_CALIBRATED; -} - -/** Returns the current bias for the compass -* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. -* Length 3. -*/ -void inv_get_compass_bias(long *bias) -{ - if (bias != NULL) { - memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); - } -} - -void inv_set_compass_bias(const long *bias, int accuracy) -{ - if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { - memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); - inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); - } - sensors.compass.accuracy = accuracy; - inv_data_builder.save.compass_accuracy = accuracy; - inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); -} - -/** Set the state of a compass disturbance -* @param[in] dist 1=disturbance, 0=no disturbance -*/ -void inv_set_compass_disturbance(int dist) -{ - inv_data_builder.compass_disturbance = dist; -} - -int inv_get_compass_disturbance(void) { - return inv_data_builder.compass_disturbance; -} -/** Sets the accel bias. -* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame -* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. -*/ -void inv_set_accel_bias(const long *bias, int accuracy) -{ - if (bias) { - if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) { - memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias)); - inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); - } - } - sensors.accel.accuracy = accuracy; - inv_data_builder.save.accel_accuracy = accuracy; - inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); -} - -/** Sets the accel accuracy. -* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. -*/ -void inv_set_accel_accuracy(int accuracy) -{ - sensors.accel.accuracy = accuracy; - inv_data_builder.save.accel_accuracy = accuracy; - inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); -} - -/** Sets the accel bias with control over which axis. -* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame -* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. -* @param[in] mask Mask to select axis to apply bias set. -*/ -void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) -{ - if (bias) { - if (mask & 1){ - inv_data_builder.save.accel_bias[0] = bias[0]; - } - if (mask & 2){ - inv_data_builder.save.accel_bias[1] = bias[1]; - } - if (mask & 4){ - inv_data_builder.save.accel_bias[2] = bias[2]; - } - - inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); - } - sensors.accel.accuracy = accuracy; - inv_data_builder.save.accel_accuracy = accuracy; - inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); -} - - -/** Sets the gyro bias -* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. -* Length 3. -* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. -*/ -void inv_set_gyro_bias(const long *bias, int accuracy) -{ - if (bias != NULL) { - if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { - memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); - inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); - } - } - sensors.gyro.accuracy = accuracy; - inv_data_builder.save.gyro_accuracy = accuracy; - - /* TODO: What should we do if there's no temperature data? */ - if (sensors.temp.calibrated[0]) - inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; - else - /* Set to 27 deg C for now until we've got a better solution. */ - inv_data_builder.save.gyro_temp = 1769472L; - inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); -} - -/* TODO: Add this information to inv_sensor_cal_t */ -/** - * Get the gyro biases and temperature record from MPL - * @param[in] bias - * Gyro bias in hardware units scaled by 2^16. - * In chip mounting frame. - * Length 3. - * @param[in] temp - * Tempearature in degrees C. - */ -void inv_get_gyro_bias(long *bias, long *temp) -{ - if (bias != NULL) - memcpy(bias, inv_data_builder.save.gyro_bias, - sizeof(inv_data_builder.save.gyro_bias)); - if (temp != NULL) - temp[0] = inv_data_builder.save.gyro_temp; -} - -/** Get Accel Bias -* @param[out] bias Accel bias where -* @param[out] temp Temperature where 1 C = 2^16 -*/ -void inv_get_accel_bias(long *bias, long *temp) -{ - if (bias != NULL) - memcpy(bias, inv_data_builder.save.accel_bias, - sizeof(inv_data_builder.save.accel_bias)); - if (temp != NULL) - temp[0] = inv_data_builder.save.accel_temp; -} - -/** - * Record new accel data for use when inv_execute_on_data() is called - * @param[in] accel accel data. - * Length 3. - * Calibrated data is in m/s^2 scaled by 2^16 in body frame. - * Raw data is in device units in chip mounting frame. - * @param[in] status - * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 - * being most accurate. - * The upper bit INV_CALIBRATED, is set if the data was calibrated - * outside MPL and it is not set if the data being passed is raw. - * Raw data should be in device units, typically in a 16-bit range. - * @param[in] timestamp - * Monotonic time stamp, for Android it's in nanoseconds. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_ACCEL; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - if ((status & INV_CALIBRATED) == 0) { - sensors.accel.raw[0] = (short)accel[0]; - sensors.accel.raw[1] = (short)accel[1]; - sensors.accel.raw[2] = (short)accel[2]; - sensors.accel.status |= INV_RAW_DATA; - inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); - } else { - sensors.accel.calibrated[0] = accel[0]; - sensors.accel.calibrated[1] = accel[1]; - sensors.accel.calibrated[2] = accel[2]; - sensors.accel.status |= INV_CALIBRATED; - sensors.accel.accuracy = status & 3; - inv_data_builder.save.accel_accuracy = status & 3; - } - sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; - sensors.accel.timestamp_prev = sensors.accel.timestamp; - sensors.accel.timestamp = timestamp; - - return INV_SUCCESS; -} - -/** Record new gyro data and calls inv_execute_on_data() if previous -* sample has not been processed. -* @param[in] gyro Data is in device units. Length 3. -* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. -* @param[out] executed Set to 1 if data processing was done. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_GYRO; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); - sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; - sensors.gyro.timestamp_prev = sensors.gyro.timestamp; - sensors.gyro.timestamp = timestamp; - inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); - - return INV_SUCCESS; -} - -/** Record new compass data for use when inv_execute_on_data() is called -* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. -* Length 3. -* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. -* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is -* not set if the data being passed is raw. Raw data should be in device units, typically -* in a 16-bit range. -* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. -* @param[out] executed Set to 1 if data processing was done. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_build_compass(const long *compass, int status, - inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_COMPASS; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - if ((status & INV_CALIBRATED) == 0) { - sensors.compass.raw[0] = (short)compass[0]; - sensors.compass.raw[1] = (short)compass[1]; - sensors.compass.raw[2] = (short)compass[2]; - inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); - sensors.compass.status |= INV_RAW_DATA; - } else { - sensors.compass.calibrated[0] = compass[0]; - sensors.compass.calibrated[1] = compass[1]; - sensors.compass.calibrated[2] = compass[2]; - sensors.compass.status |= INV_CALIBRATED; - sensors.compass.accuracy = status & 3; - inv_data_builder.save.compass_accuracy = status & 3; - } - sensors.compass.timestamp_prev = sensors.compass.timestamp; - sensors.compass.timestamp = timestamp; - sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; - - return INV_SUCCESS; -} - -/** Record new temperature data for use when inv_execute_on_data() is called. - * @param[in] temp Temperature data in q16 format. - * @param[in] timestamp Monotonic time stamp; for Android it's in - * nanoseconds. -* @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_TEMPERATURE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - sensors.temp.calibrated[0] = temp; - sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; - sensors.temp.timestamp_prev = sensors.temp.timestamp; - sensors.temp.timestamp = timestamp; - /* TODO: Apply scale, remove offset. */ - - return INV_SUCCESS; -} -/** quaternion data -* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. -* Real part first. Length 4. -* @param[in] status number of axis, 16-bit or 32-bit -* @param[in] timestamp -* @param[in] timestamp Monotonic time stamp; for Android it's in -* nanoseconds. -* @param[out] executed Set to 1 if data processing was done. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) -{ -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_QUAT; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); - fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); - } -#endif - - memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); - sensors.quat.timestamp = timestamp; - sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; - sensors.quat.status |= (INV_BIAS_APPLIED & status); - - return INV_SUCCESS; -} - -/** This should be called when the accel has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_accel_was_turned_off() -{ - sensors.accel.status = 0; -} - -/** This should be called when the compass has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_compass_was_turned_off() -{ - sensors.compass.status = 0; -} - -/** This should be called when the quaternion data from the DMP has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_quaternion_sensor_was_turned_off(void) -{ - sensors.quat.status = 0; -} - -/** This should be called when the gyro has been turned off. This is so -* that we will know if the data is contiguous. -*/ -void inv_gyro_was_turned_off() -{ - sensors.gyro.status = 0; -} - -/** This should be called when the temperature sensor has been turned off. - * This is so that we will know if the data is contiguous. - */ -void inv_temperature_was_turned_off() -{ - sensors.temp.status = 0; -} - -/** Registers to receive a callback when there is new sensor data. -* @internal -* @param[in] func Function pointer to receive callback when there is new sensor data -* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority -* numbers must be unique. -* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be -* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = -* gyro data, INV_MAG_NEW = compass data. So passing in -* INV_ACCEL_NEW | INV_MAG_NEW, a -* callback would be generated if there was new magnetomer data OR new accel data. -*/ -inv_error_t inv_register_data_cb( - inv_error_t (*func)(struct inv_sensor_cal_t *data), - int priority, int sensor_type) -{ - inv_error_t result = INV_SUCCESS; - int kk, nn; - - // Make sure we haven't registered this function already - // Or used the same priority - for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if ((inv_data_builder.process[kk].func == func) || - (inv_data_builder.process[kk].priority == priority)) { - return INV_ERROR_INVALID_PARAMETER; //fixme give a warning - } - } - - // Make sure we have not filled up our number of allowable callbacks - if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { - kk = 0; - if (inv_data_builder.num_cb != 0) { - // set kk to be where this new callback goes in the array - while ((kk < inv_data_builder.num_cb) && - (inv_data_builder.process[kk].priority < priority)) { - kk++; - } - if (kk != inv_data_builder.num_cb) { - // We need to move the others - for (nn = inv_data_builder.num_cb; nn > kk; --nn) { - inv_data_builder.process[nn] = - inv_data_builder.process[nn - 1]; - } - } - } - // Add new callback - inv_data_builder.process[kk].func = func; - inv_data_builder.process[kk].priority = priority; - inv_data_builder.process[kk].data_required = sensor_type; - inv_data_builder.num_cb++; - } else { - MPL_LOGE("Unable to add feature callback as too many were already registered\n"); - result = INV_ERROR_MEMORY_EXAUSTED; - } - - return result; -} - -/** Unregisters the callback that happens when new sensor data is received. -* @internal -* @param[in] func Function pointer to receive callback when there is new sensor data -* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority -* numbers must be unique. -* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be -* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = -* gyro data, INV_MAG_NEW = compass data. So passing in -* INV_ACCEL_NEW | INV_MAG_NEW, a -* callback would be generated if there was new magnetomer data OR new accel data. -*/ -inv_error_t inv_unregister_data_cb( - inv_error_t (*func)(struct inv_sensor_cal_t *data)) -{ - int kk, nn; - - for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if (inv_data_builder.process[kk].func == func) { - // Delete this callback - for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { - inv_data_builder.process[nn - 1] = - inv_data_builder.process[nn]; - } - inv_data_builder.num_cb--; - return INV_SUCCESS; - } - } - - return INV_SUCCESS; // We did not find the callback -} - -/** After at least one of inv_build_gyro(), inv_build_accel(), or -* inv_build_compass() has been called, this function should be called. -* It will process the data it has received and update all the internal states -* and features that have been turned on. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_execute_on_data(void) -{ - inv_error_t result, first_error; - int kk; - -#ifdef INV_PLAYBACK_DBG - if (inv_data_builder.debug_mode == RD_RECORD) { - int type = PLAYBACK_DBG_TYPE_EXECUTE; - fwrite(&type, sizeof(type), 1, inv_data_builder.file); - } -#endif - // Determine what new data we have - inv_data_builder.mode = 0; - if (sensors.gyro.status & INV_NEW_DATA) - inv_data_builder.mode |= INV_GYRO_NEW; - if (sensors.accel.status & INV_NEW_DATA) - inv_data_builder.mode |= INV_ACCEL_NEW; - if (sensors.compass.status & INV_NEW_DATA) - inv_data_builder.mode |= INV_MAG_NEW; - if (sensors.temp.status & INV_NEW_DATA) - inv_data_builder.mode |= INV_TEMP_NEW; - if (sensors.quat.status & INV_QUAT_NEW) - inv_data_builder.mode |= INV_QUAT_NEW; - - first_error = INV_SUCCESS; - - for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) { - result = inv_data_builder.process[kk].func(&sensors); - if (result && !first_error) { - first_error = result; - } - } - } - - inv_set_contiguous(); - - return first_error; -} - -/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. -* -*/ -static void inv_set_contiguous(void) -{ - inv_time_t current_time = 0; - if (sensors.gyro.status & INV_NEW_DATA) { - sensors.gyro.status |= INV_CONTIGUOUS; - current_time = sensors.gyro.timestamp; - } - if (sensors.accel.status & INV_NEW_DATA) { - sensors.accel.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.accel.timestamp); - } - if (sensors.compass.status & INV_NEW_DATA) { - sensors.compass.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.compass.timestamp); - } - if (sensors.temp.status & INV_NEW_DATA) { - sensors.temp.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.temp.timestamp); - } - if (sensors.quat.status & INV_NEW_DATA) { - sensors.quat.status |= INV_CONTIGUOUS; - current_time = MAX(current_time, sensors.quat.timestamp); - } - -#if 0 - /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() - * type functions. This is just in case that breaks down. We make sure - * all the data is within 2 seconds of the newest piece of data*/ - if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) - inv_gyro_was_turned_off(); - if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) - inv_accel_was_turned_off(); - if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) - inv_compass_was_turned_off(); - /* TODO: Temperature might not need to be read this quickly. */ - if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) - inv_temperature_was_turned_off(); -#endif - - /* clear bits */ - sensors.gyro.status &= ~INV_NEW_DATA; - sensors.accel.status &= ~INV_NEW_DATA; - sensors.compass.status &= ~INV_NEW_DATA; - sensors.temp.status &= ~INV_NEW_DATA; - sensors.quat.status &= ~INV_NEW_DATA; -} - -/** Gets a whole set of accel data including data, accuracy and timestamp. - * @param[out] data Accel Data where 1g = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - if (data != NULL) { - memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); - } - if (timestamp != NULL) { - *timestamp = sensors.accel.timestamp; - } - if (accuracy != NULL) { - *accuracy = sensors.accel.accuracy; - } -} - -/** Gets a whole set of gyro data including data, accuracy and timestamp. - * @param[out] data Gyro Data where 1 dps = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); - if (timestamp != NULL) { - *timestamp = sensors.gyro.timestamp; - } - if (accuracy != NULL) { - *accuracy = sensors.gyro.accuracy; - } -} - -/** Gets a whole set of gyro raw data including data, accuracy and timestamp. - * @param[out] data Gyro Data where 1 dps = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); - if (timestamp != NULL) { - *timestamp = sensors.gyro.timestamp; - } - if (accuracy != NULL) { - *accuracy = sensors.gyro.accuracy; - } -} - -/** Get's latest gyro data. -* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. -*/ -void inv_get_gyro(long *gyro) -{ - memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); -} - -/** Gets a whole set of compass data including data, accuracy and timestamp. - * @param[out] data Compass Data where 1 uT = 2^16 - * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. - * @param[out] timestamp The timestamp of the data sample. -*/ -void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) -{ - memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); - if (timestamp != NULL) { - *timestamp = sensors.compass.timestamp; - } - if (accuracy != NULL) { - if (inv_data_builder.compass_disturbance) - *accuracy = 0; - else - *accuracy = sensors.compass.accuracy; - } -} - -/** Gets a whole set of temperature data including data, accuracy and timestamp. - * @param[out] data Temperature data where 1 degree C = 2^16 - * @param[out] accuracy 0 to 3, where 3 is most accurate. - * @param[out] timestamp The timestamp of the data sample. - */ -void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) -{ - data[0] = sensors.temp.calibrated[0]; - if (timestamp) - *timestamp = sensors.temp.timestamp; - if (accuracy) - *accuracy = sensors.temp.accuracy; -} - -/** Returns accuracy of gyro. - * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. -*/ -int inv_get_gyro_accuracy(void) -{ - return sensors.gyro.accuracy; -} - -/** Returns accuracy of compass. - * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. -*/ -int inv_get_mag_accuracy(void) -{ - if (inv_data_builder.compass_disturbance) - return 0; - return sensors.compass.accuracy; -} - -/** Returns accuracy of accel. - * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. -*/ -int inv_get_accel_accuracy(void) -{ - return sensors.accel.accuracy; -} - -inv_error_t inv_get_gyro_orient(int *orient) -{ - *orient = sensors.gyro.orientation; - return 0; -} - -inv_error_t inv_get_accel_orient(int *orient) -{ - *orient = sensors.accel.orientation; - return 0; -} - - -/** - * @} - */ diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h deleted file mode 100644 index c8c18cf..0000000 --- a/60xx/libsensors_iio/software/core/mllite/data_builder.h +++ /dev/null @@ -1,240 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_DATA_BUILDER_H__ -#define INV_DATA_BUILDER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -// Uncomment this flag to enable playback debug and record or playback scenarios -//#define INV_PLAYBACK_DBG - -/** This is a new sample of accel data */ -#define INV_ACCEL_NEW 1 -/** This is a new sample of gyro data */ -#define INV_GYRO_NEW 2 -/** This is a new sample of compass data */ -#define INV_MAG_NEW 4 -/** This is a new sample of temperature data */ -#define INV_TEMP_NEW 8 -/** This is a new sample of quaternion data */ -#define INV_QUAT_NEW 16 -/** Set if quaternion is 6-axis from DMP */ -#define INV_QUAT_6AXIS 1024 -/** Set if quaternion is 3-axis from DMP */ -#define INV_QUAT_3AXIS 4096 - -/** Set if the data is contiguous. Typically not set if a sample was skipped */ -#define INV_CONTIGUOUS 16 -/** Set if the calibrated data has been solved for */ -#define INV_CALIBRATED 32 -/* INV_NEW_DATA set for a new set of data, cleared if not available. */ -#define INV_NEW_DATA 64 -/* Set if raw data exists */ -#define INV_RAW_DATA 128 -/* Set if the sensor is on */ -#define INV_SENSOR_ON 256 -/* Set if quaternion has bias correction applied */ -#define INV_BIAS_APPLIED 512 - -#define INV_PRIORITY_MOTION_NO_MOTION 100 -#define INV_PRIORITY_GYRO_TC 150 -#define INV_PRIORITY_QUATERNION_GYRO_ACCEL 200 -#define INV_PRIORITY_QUATERNION_NO_GYRO 250 -#define INV_PRIORITY_MAGNETIC_DISTURBANCE 300 -#define INV_PRIORITY_HEADING_FROM_GYRO 350 -#define INV_PRIORITY_COMPASS_BIAS_W_GYRO 375 -#define INV_PRIORITY_COMPASS_VECTOR_CAL 400 -#define INV_PRIORITY_COMPASS_ADV_BIAS 500 -#define INV_PRIORITY_9_AXIS_FUSION 600 -#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS 700 -#define INV_PRIORITY_QUATERNION_ACCURACY 750 -#define INV_PRIORITY_RESULTS_HOLDER 800 -#define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850 -#define INV_PRIORITY_HAL_OUTPUTS 900 -#define INV_PRIORITY_GLYPH 950 -#define INV_PRIORITY_SHAKE 975 -#define INV_PRIORITY_SM 1000 - -struct inv_single_sensor_t { - /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when - * the rotation matrix could be thought of only having elements of 0,1,-1. - * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign. - * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row. - * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row. - * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row. - */ - int orientation; - /** The raw data in raw data units in the mounting frame */ - short raw[3]; - /** Raw data in body frame */ - long raw_scaled[3]; - /** Calibrated data */ - long calibrated[3]; - long sensitivity; - /** Sample rate in microseconds */ - long sample_rate_us; - long sample_rate_ms; - /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample - * skipped due to power savings turning off this sensor. - * INV_NEW_DATA set for a new set of data, cleared if not available. - * INV_CALIBRATED_SET if calibrated data has been solved for */ - int status; - /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */ - int accuracy; - inv_time_t timestamp; - inv_time_t timestamp_prev; - /** Bandwidth in Hz */ - int bandwidth; -}; -struct inv_quat_sensor_t { - long raw[4]; - /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample - * skipped due to power savings turning off this sensor. - * INV_NEW_DATA set for a new set of data, cleared if not available. - * INV_CALIBRATED_SET if calibrated data has been solved for */ - int status; - inv_time_t timestamp; - inv_time_t timestamp_prev; - long sample_rate_us; - long sample_rate_ms; -}; - -struct inv_sensor_cal_t { - struct inv_single_sensor_t gyro; - struct inv_single_sensor_t accel; - struct inv_single_sensor_t compass; - struct inv_single_sensor_t temp; - struct inv_quat_sensor_t quat; - /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate - * which data is a new sample as these data points may have different sample rates. - */ - int status; -}; - -// Useful for debug record and playback -typedef enum { - RD_NO_DEBUG, - RD_RECORD, - RD_PLAYBACK -} rd_dbg_mode; - -typedef enum { - PLAYBACK_DBG_TYPE_GYRO, - PLAYBACK_DBG_TYPE_ACCEL, - PLAYBACK_DBG_TYPE_COMPASS, - PLAYBACK_DBG_TYPE_TEMPERATURE, - PLAYBACK_DBG_TYPE_EXECUTE, - PLAYBACK_DBG_TYPE_A_ORIENT, - PLAYBACK_DBG_TYPE_G_ORIENT, - PLAYBACK_DBG_TYPE_C_ORIENT, - PLAYBACK_DBG_TYPE_A_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_C_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_G_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_GYRO_OFF, - PLAYBACK_DBG_TYPE_ACCEL_OFF, - PLAYBACK_DBG_TYPE_COMPASS_OFF, - PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE, - PLAYBACK_DBG_TYPE_QUAT - -} inv_rd_dbg_states; - -/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/ -#define INV_MAX_DATA_CB 20 - -#ifdef INV_PLAYBACK_DBG -#include <stdio.h> -void inv_turn_on_data_logging(FILE *file); -void inv_turn_off_data_logging(); -#endif - -void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity); -void inv_set_accel_orientation_and_scale(int orientation, - long sensitivity); -void inv_set_compass_orientation_and_scale(int orientation, - long sensitivity); -void inv_set_gyro_sample_rate(long sample_rate_us); -void inv_set_compass_sample_rate(long sample_rate_us); -void inv_set_quat_sample_rate(long sample_rate_us); -void inv_set_accel_sample_rate(long sample_rate_us); -void inv_set_gyro_bandwidth(int bandwidth_hz); -void inv_set_accel_bandwidth(int bandwidth_hz); -void inv_set_compass_bandwidth(int bandwidth_hz); - -void inv_get_gyro_sample_rate_ms(long *sample_rate_ms); -void inv_get_accel_sample_rate_ms(long *sample_rate_ms); -void inv_get_compass_sample_rate_ms(long *sample_rate_ms); - -inv_error_t inv_register_data_cb(inv_error_t (*func) - (struct inv_sensor_cal_t * data), int priority, - int sensor_type); -inv_error_t inv_unregister_data_cb(inv_error_t (*func) - (struct inv_sensor_cal_t * data)); - -inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp); -inv_error_t inv_build_compass(const long *compass, int status, - inv_time_t timestamp); -inv_error_t inv_build_accel(const long *accel, int status, - inv_time_t timestamp); -inv_error_t inv_build_temp(const long temp, inv_time_t timestamp); -inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp); -inv_error_t inv_execute_on_data(void); - -void inv_get_compass_bias(long *bias); - -void inv_set_compass_bias(const long *bias, int accuracy); -void inv_set_compass_disturbance(int dist); -void inv_set_gyro_bias(const long *bias, int accuracy); -void inv_set_accel_bias(const long *bias, int accuracy); -void inv_set_accel_accuracy(int accuracy); -void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); - -void inv_get_gyro_bias(long *bias, long *temp); -void inv_get_accel_bias(long *bias, long *temp); - -void inv_gyro_was_turned_off(void); -void inv_accel_was_turned_off(void); -void inv_compass_was_turned_off(void); -void inv_quaternion_sensor_was_turned_off(void); -inv_error_t inv_init_data_builder(void); -long inv_get_gyro_sensitivity(void); -long inv_get_accel_sensitivity(void); -long inv_get_compass_sensitivity(void); - -void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp); -void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp); -void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); -void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp); - -void inv_get_gyro(long *gyro); - -int inv_get_gyro_accuracy(void); -int inv_get_accel_accuracy(void); -int inv_get_mag_accuracy(void); - -int inv_get_compass_on(void); -int inv_get_gyro_on(void); -int inv_get_accel_on(void); - -inv_time_t inv_get_last_timestamp(void); -int inv_get_compass_disturbance(void); - -//New DMP Cal Functions -inv_error_t inv_get_gyro_orient(int *orient); -inv_error_t inv_get_accel_orient(int *orient); - -// internal -int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts); - -#ifdef __cplusplus -} -#endif - -#endif /* INV_DATA_BUILDER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c deleted file mode 100644 index caa1db7..0000000 --- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c +++ /dev/null @@ -1,497 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/**
- * @defgroup HAL_Outputs hal_outputs
- * @brief Motion Library - HAL Outputs
- * Sets up common outputs for HAL
- *
- * @{
- * @file hal_outputs.c
- * @brief HAL Outputs.
- */
-
-#include <string.h>
-
-#include "hal_outputs.h"
-#include "log.h"
-#include "ml_math_func.h"
-#include "mlmath.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "results_holder.h"
-
-struct hal_output_t {
- int accuracy_mag; /**< Compass accuracy */
-// int accuracy_gyro; /**< Gyro Accuracy */
-// int accuracy_accel; /**< Accel Accuracy */
- int accuracy_quat; /**< quat Accuracy */
-
- inv_time_t nav_timestamp;
- inv_time_t gam_timestamp;
-// inv_time_t accel_timestamp;
- inv_time_t mag_timestamp;
- long nav_quat[4];
- int gyro_status;
- int accel_status;
- int compass_status;
- int nine_axis_status;
- inv_biquad_filter_t lp_filter[3];
- float compass_float[3];
- long linear_acceleration_sample_rate_us;
- long gravity_sample_rate_us;
-};
-
-static struct hal_output_t hal_out;
-
-void inv_set_linear_acceleration_sample_rate(long sample_rate_us)
-{
- hal_out.linear_acceleration_sample_rate_us = sample_rate_us;
-}
-
-void inv_set_gravity_sample_rate(long sample_rate_us)
-{
- hal_out.gravity_sample_rate_us = sample_rate_us;
-}
-
-/** Acceleration (m/s^2) in body frame.
-* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it
-* should return a vector of magnitude near 9.81 m/s^2
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_accel().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- int status;
- /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16.
- * So this 9.80665 / 2^16 */
-#define ACCEL_CONVERSION 0.000149637603759766f
- long accel[3];
- inv_get_accel_set(accel, accuracy, timestamp);
- values[0] = accel[0] * ACCEL_CONVERSION;
- values[1] = accel[1] * ACCEL_CONVERSION;
- values[2] = accel[2] * ACCEL_CONVERSION;
- if (hal_out.accel_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-/** Linear Acceleration (m/s^2) in Body Frame.
-* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show
-* accel biases while at rest.
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_accel().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gravity[3], accel[3];
- inv_time_t timestamp1;
-
- inv_get_accel_set(accel, accuracy, ×tamp1);
- inv_get_gravity(gravity);
- accel[0] -= gravity[0] >> 14;
- accel[1] -= gravity[1] >> 14;
- accel[2] -= gravity[2] >> 14;
- values[0] = accel[0] * ACCEL_CONVERSION;
- values[1] = accel[1] * ACCEL_CONVERSION;
- values[2] = accel[2] * ACCEL_CONVERSION;
-
- return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp);
-}
-
-/** Gravity vector (m/s^2) in Body Frame.
-* @param[out] values Gravity vector in body frame, length 3, (m/s^2)
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_accel().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gravity(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gravity[3];
-
- *accuracy = (int8_t) hal_out.accuracy_quat;
- inv_get_gravity(gravity);
- values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION;
- values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION;
- values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION;
- return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp);
-}
-
-/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16.
- * So this is: pi / 2^16 / 180 */
-#define GYRO_CONVERSION 2.66316109007924e-007f
-
-/** Gyroscope calibrated data (rad/s) in body frame.
-* @param[out] values Rotation Rate in rad/sec.
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_gyro().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gyro[3];
- int status;
-
- inv_get_gyro_set(gyro, accuracy, timestamp);
- values[0] = gyro[0] * GYRO_CONVERSION;
- values[1] = gyro[1] * GYRO_CONVERSION;
- values[2] = gyro[2] * GYRO_CONVERSION;
- if (hal_out.gyro_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-/** Gyroscope raw data (rad/s) in body frame.
-* @param[out] values Rotation Rate in rad/sec.
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to
-* inv_build_gyro().
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- long gyro[3];
- int status;
-
- inv_get_gyro_set_raw(gyro, accuracy, timestamp);
- values[0] = gyro[0] * GYRO_CONVERSION;
- values[1] = gyro[1] * GYRO_CONVERSION;
- values[2] = gyro[2] * GYRO_CONVERSION;
- if (hal_out.gyro_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-/**
-* This corresponds to Sensor.TYPE_ROTATION_VECTOR.
-* The rotation vector represents the orientation of the device as a combination
-* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$
-* around an axis {x, y, z}. <br>
-* The three elements of the rotation vector are
-* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation
-* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is
-* equal to the direction of the axis of rotation.
-*
-* The three elements of the rotation vector are equal to the last three components of a unit quaternion
-* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2).
-*
-* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor.
-* The reference coordinate system is defined as a direct orthonormal basis, where:
-
- -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East).
- -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.
- -Z points towards the sky and is perpendicular to the ground.
-* @param[out] values Length 4.
-* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
-* @param[out] timestamp Timestamp. In (ns) for Android.
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- *accuracy = (int8_t) hal_out.accuracy_quat;
- *timestamp = hal_out.nav_timestamp;
-
- if (hal_out.nav_quat[0] >= 0) {
- values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
- values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
- values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
- values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
- } else {
- values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30;
- values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30;
- values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30;
- values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30;
- }
- values[4] = inv_get_heading_confidence_interval();
-
- return hal_out.nine_axis_status;
-}
-
-
-/** Compass data (uT) in body frame.
-* @param[out] values Compass data in (uT), length 3. May be calibrated by having
-* biases removed and sensitivity adjusted
-* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate
-* @param[out] timestamp Timestamp. In (ns) for Android.
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- int status;
- /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
- * So this is: 1 / 2^16*/
-//#define COMPASS_CONVERSION 1.52587890625e-005f
- int i;
-
- *timestamp = hal_out.mag_timestamp;
- *accuracy = (int8_t) hal_out.accuracy_mag;
-
- for (i=0; i<3; i++) {
- values[i] = hal_out.compass_float[i];
- }
- if (hal_out.compass_status & INV_NEW_DATA)
- status = 1;
- else
- status = 0;
- return status;
-}
-
-static void inv_get_rotation(float r[3][3])
-{
- long rot[9];
- float conv = 1.f / (1L<<30);
-
- inv_quaternion_to_rotation(hal_out.nav_quat, rot);
- r[0][0] = rot[0]*conv;
- r[0][1] = rot[1]*conv;
- r[0][2] = rot[2]*conv;
- r[1][0] = rot[3]*conv;
- r[1][1] = rot[4]*conv;
- r[1][2] = rot[5]*conv;
- r[2][0] = rot[6]*conv;
- r[2][1] = rot[7]*conv;
- r[2][2] = rot[8]*conv;
-}
-
-static void google_orientation(float *g)
-{
- float rad2deg = (float)(180.0 / M_PI);
- float R[3][3];
-
- inv_get_rotation(R);
-
- g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg;
- g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg;
- g[2] = asinf ( R[2][0]) * rad2deg;
- if (g[0] < 0)
- g[0] += 360;
-}
-
-
-/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees.
-* @param[out] values Length 3, Degrees.<br>
-* - values[0]: Azimuth, angle between the magnetic north direction
-* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West<br>
-* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values
-* when the z-axis moves toward the y-axis.<br>
-* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive
-* values when the x-axis moves toward the z-axis.<br>
-*
-* @note This definition is different from yaw, pitch and roll used in aviation
-* where the X axis is along the long side of the plane (tail to nose).
-* Note: This sensor type exists for legacy reasons, please use getRotationMatrix()
-* in conjunction with remapCoordinateSystem() and getOrientation() to compute
-* these values instead.
-* Important note: For historical reasons the roll angle is positive in the
-* clockwise direction (mathematically speaking, it should be positive in
-* the counter-clockwise direction).
-* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate.
-* @param[out] timestamp The timestamp for this sensor.
-* @return Returns 1 if the data was updated or 0 if it was not updated.
-*/
-int inv_get_sensor_type_orientation(float *values, int8_t *accuracy,
- inv_time_t * timestamp)
-{
- *accuracy = (int8_t) hal_out.accuracy_quat;
- *timestamp = hal_out.nav_timestamp;
-
- google_orientation(values);
-
- return hal_out.nine_axis_status;
-}
-
-/** Main callback to generate HAL outputs. Typically not called by library users.
-* @param[in] sensor_cal Input variable to take sensor data whenever there is new
-* sensor data.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal)
-{
- int use_sensor = 0;
- long sr = 1000;
- long compass[3];
- int8_t accuracy;
- int i;
- (void) sensor_cal;
-
- inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat,
- &hal_out.nav_timestamp);
- hal_out.gyro_status = sensor_cal->gyro.status;
- hal_out.accel_status = sensor_cal->accel.status;
- hal_out.compass_status = sensor_cal->compass.status;
-
- // Find the highest sample rate and tie generating 9-axis to that one.
- if (sensor_cal->gyro.status & INV_SENSOR_ON) {
- sr = sensor_cal->gyro.sample_rate_ms;
- use_sensor = 0;
- }
- if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) {
- sr = sensor_cal->accel.sample_rate_ms;
- use_sensor = 1;
- }
- if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) {
- sr = sensor_cal->compass.sample_rate_ms;
- use_sensor = 2;
- }
- if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) {
- sr = sensor_cal->quat.sample_rate_ms;
- use_sensor = 3;
- }
-
- // Only output 9-axis if all 9 sensors are on.
- if (sensor_cal->quat.status & INV_SENSOR_ON) {
- // If quaternion sensor is on, gyros are not required as quaternion already has that part
- if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
- use_sensor = -1;
- }
- } else {
- if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) {
- use_sensor = -1;
- }
- }
-
- switch (use_sensor) {
- case 0:
- hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->gyro.timestamp;
- break;
- case 1:
- hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->accel.timestamp;
- break;
- case 2:
- hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->compass.timestamp;
- break;
- case 3:
- hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0;
- hal_out.nav_timestamp = sensor_cal->quat.timestamp;
- break;
- default:
- hal_out.nine_axis_status = 0; // Don't output quaternion related info
- break;
- }
-
- /* Converts fixed point to uT. Fixed point has 1 uT = 2^16.
- * So this is: 1 / 2^16*/
- #define COMPASS_CONVERSION 1.52587890625e-005f
-
- inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) );
- hal_out.accuracy_mag = (int ) accuracy;
-
- for (i=0; i<3; i++) {
- if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) ==
- INV_NEW_DATA ) {
- // set the state variables to match output with input
- inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]);
- }
-
- if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) ==
- (INV_NEW_DATA | INV_RAW_DATA) ) {
-
- hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i],
- (float ) compass[i]) * COMPASS_CONVERSION;
-
- } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) {
- hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION;
- }
-
- }
- return INV_SUCCESS;
-}
-
-/** Turns off generation of HAL outputs.
-* @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_stop_hal_outputs(void)
-{
- inv_error_t result;
- result = inv_unregister_data_cb(inv_generate_hal_outputs);
- return result;
-}
-
-/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs()
-* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs().
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_start_hal_outputs(void)
-{
- inv_error_t result;
- result =
- inv_register_data_cb(inv_generate_hal_outputs,
- INV_PRIORITY_HAL_OUTPUTS,
- INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW);
- return result;
-}
-/* file name: lowPassFilterCoeff_1_6.c */
-float compass_low_pass_filter_coeff[5] =
-{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f};
-
-/** Initializes hal outputs class. This is called automatically by the
-* enable function. It may be called any time the feature is enabled, but
-* is typically not needed to be called by outside callers.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_hal_outputs(void)
-{
- int i;
- memset(&hal_out, 0, sizeof(hal_out));
- for (i=0; i<3; i++) {
- inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff);
- }
-
- return INV_SUCCESS;
-}
-
-/** Turns on creation and storage of HAL type results.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_enable_hal_outputs(void)
-{
- inv_error_t result;
-
- // don't need to check the result for inv_init_hal_outputs
- // since it's always INV_SUCCESS
- inv_init_hal_outputs();
-
- result = inv_register_mpl_start_notification(inv_start_hal_outputs);
- return result;
-}
-
-/** Turns off creation and storage of HAL type results.
-*/
-inv_error_t inv_disable_hal_outputs(void)
-{
- inv_error_t result;
-
- inv_stop_hal_outputs(); // Ignore error if we have already stopped this
- result = inv_unregister_mpl_start_notification(inv_start_hal_outputs);
- return result;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h deleted file mode 100644 index 41b72c6..0000000 --- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_HAL_OUTPUTS_H__ -#define INV_HAL_OUTPUTS_H__ - -#ifdef __cplusplus -extern "C" { -#endif - - int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, - inv_time_t * timestamp); - - int inv_get_sensor_type_linear_acceleration(float *values, - int8_t *accuracy, - inv_time_t * timestamp); - int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, - inv_time_t * timestamp); - - inv_error_t inv_enable_hal_outputs(void); - inv_error_t inv_disable_hal_outputs(void); - inv_error_t inv_init_hal_outputs(void); - inv_error_t inv_start_hal_outputs(void); - inv_error_t inv_stop_hal_outputs(void); - - // Set data rates for virtual sensors - void inv_set_linear_acceleration_sample_rate(long sample_rate_us); - void inv_set_gravity_sample_rate(long sample_rate_us); - -#ifdef __cplusplus -} -#endif - -#endif // INV_HAL_OUTPUTS_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h deleted file mode 100644 index fb8e12b..0000000 --- a/60xx/libsensors_iio/software/core/mllite/invensense.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -/** - * Main header file for Invensense's basic library. - */ - -#include "data_builder.h" -#include "hal_outputs.h" -#include "message_layer.h" -#include "mlmath.h" -#include "ml_math_func.h" -#include "mpl.h" -#include "results_holder.h" -#include "start_manager.h" -#include "storage_manager.h" -#include "log.h" -#include "mlinclude.h" -//#include "..\HAL\include\mlos.h" diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c deleted file mode 100644 index 72940f7..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c +++ /dev/null @@ -1,279 +0,0 @@ -/* - $License: - Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id:$ - * - *****************************************************************************/ - -/** - * @defgroup ML_LOAD_DMP - * - * @{ - * @file ml_load_dmp.c - * @brief functions for writing dmp firmware. - */ -#include <stdio.h> - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-loaddmp" - -#include "ml_load_dmp.h" -#include "mlos.h" - -#define LOADDMP_LOG MPL_LOGI -#define LOADDMP_LOG MPL_LOGI - -#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) -#define DMP_CODE_SIZE 3065 - -static const unsigned char dmpMemory[DMP_CODE_SIZE] = { - /* bank # 0 */ - 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, - 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, - 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, - 0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, - 0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, - 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, - 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, - 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, - 0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, - 0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, - /* bank # 1 */ - 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, - 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, - 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, - 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, - 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, - 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, - 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, - 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, - 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, - /* bank # 2 */ - 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, - 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, - 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, - 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - /* bank # 3 */ - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, - 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - /* bank # 4 */ - 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, - 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, - 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, - 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, - 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, - 0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, - 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, - 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, - 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, - 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, - 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, - 0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, - 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, - 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, - 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, - 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, - /* bank # 5 */ - 0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, - 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, - 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, - 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, - 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, - 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, - 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, - 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, - 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, - 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, - 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, - 0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, - 0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, - 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, - 0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, - 0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, - /* bank # 6 */ - 0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, - 0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, - 0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, - 0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, - 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, - 0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, - 0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, - 0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, - 0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, - 0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, - 0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, - 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, - 0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, - 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, - 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, - 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, - /* bank # 7 */ - 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, - 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, - 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, - 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, - 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, - 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, - 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, - 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, - 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, - 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, - 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, - 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, - 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, - 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, - 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, - 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, - /* bank # 8 */ - 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, - 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, - 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, - 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, - 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, - 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, - 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 0xb4, 0xb0, - 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, - 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, - 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, - 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, - 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, - 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, - 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, - 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, - 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, - /* bank # 9 */ - 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, - 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, - 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, - 0x88, 0x2d, 0x55, 0x7d, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, - 0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, - 0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, - 0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, - 0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, - 0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, - 0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, - 0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, - 0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, - 0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, - 0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, - 0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, - 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, - /* bank # 10 */ - 0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, - 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, - 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, - 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, - 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, - 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, - 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, - 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, - 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, - 0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, - 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, - 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, - 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, - 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, - 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, - 0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, - /* bank # 11 */ - 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, - 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, - 0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, - 0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, - 0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, - 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, - 0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, - 0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, - 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, - 0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, - 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, - 0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, - 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, - 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, - 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, - 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff -}; - -#define DMP_VERSION (dmpMemory) - -inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) -{ - inv_error_t result = INV_SUCCESS; - int bytesWritten = 0; - - if (len <= 0) { - MPL_LOGE("Nothing to write"); - return INV_ERROR_FILE_WRITE; - } else { - MPL_LOGI("dmp firmware size to write = %d", len); - } - if (fd == NULL) { - return INV_ERROR_FILE_OPEN; - } - bytesWritten = fwrite(dmp, 1, len, fd); - if (bytesWritten != len) { - MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", - bytesWritten, len); - result = INV_ERROR_FILE_WRITE; - } else { - MPL_LOGI("Bytes written = %d", bytesWritten); - } - return result; -} - -inv_error_t inv_load_dmp(FILE *fd) -{ - inv_error_t result = INV_SUCCESS; - result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE); - return result; -} - -/** - * @} - */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h deleted file mode 100644 index 4d98692..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - $License: - Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -#ifndef INV_LOAD_DMP_H -#define INV_LOAD_DMP_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Includes. -*/ -#include "mltypes.h" - -/* - APIs -*/ -inv_error_t inv_load_dmp(FILE *fd); - -#ifdef __cplusplus -} -#endif -#endif /* INV_LOAD_DMP_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c deleted file mode 100644 index b4c4249..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c +++ /dev/null @@ -1,353 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup ML_STORED_DATA - * - * @{ - * @file ml_stored_data.c - * @brief functions for reading and writing stored data sets. - * Typically, these functions process stored calibration data. - */ - -#include <stdio.h> - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-storeload" - -#include "ml_stored_data.h" -#include "storage_manager.h" -#include "mlos.h" - -#define LOADCAL_DEBUG 0 -#define STORECAL_DEBUG 0 - -#define DEFAULT_KEY 29681 - -#define STORECAL_LOG MPL_LOGI -#define LOADCAL_LOG MPL_LOGI - -inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) -{ - FILE *fp; - inv_error_t result = INV_SUCCESS; - size_t fsize; - - fp = fopen(MLCAL_FILE,"rb"); - if (fp == NULL) { - MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - - // obtain file size - fseek (fp, 0 , SEEK_END); - fsize = ftell (fp); - rewind (fp); - - *calData = (unsigned char *)inv_malloc(fsize); - if (*calData==NULL) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", fsize); - fclose(fp); - return INV_ERROR_MEMORY_EXAUSTED; - } - - *bytesRead = fread(*calData, 1, fsize, fp); - if (*bytesRead != fsize) { - MPL_LOGE("bytes read (%d) don't match file size (%d)\n", - *bytesRead, fsize); - result = INV_ERROR_FILE_READ; - goto read_cal_end; - } - else { - MPL_LOGI("Bytes read = %d", *bytesRead); - } - -read_cal_end: - fclose(fp); - return result; -} - -inv_error_t inv_write_cal(unsigned char *cal, size_t len) -{ - FILE *fp; - int bytesWritten; - inv_error_t result = INV_SUCCESS; - - if (len <= 0) { - MPL_LOGE("Nothing to write"); - return INV_ERROR_FILE_WRITE; - } - else { - MPL_LOGI("cal data size to write = %d", len); - } - fp = fopen(MLCAL_FILE,"wb"); - if (fp == NULL) { - MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - bytesWritten = fwrite(cal, 1, len, fp); - if (bytesWritten != len) { - MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", - bytesWritten, len); - result = INV_ERROR_FILE_WRITE; - } - else { - MPL_LOGI("Bytes written = %d", bytesWritten); - } - fclose(fp); - return result; -} - -/** - * @brief Loads a type 0 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature compensation : temperature data points, - * - temperature compensation : gyro biases data points for X, Y, - * and Z axes. - * - accel biases for X, Y, Z axes. - * This calibration data is produced internally by the MPL and its - * size is 2777 bytes (header and checksum included). - * Calibration format type 1 is currently used for ITG3500 - * - * @pre inv_init_storage_manager() - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) -{ - inv_error_t result; - - LOADCAL_LOG("Entering inv_load_cal_V0\n"); - - /*if (len != expLen) { - MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", - expLen, len); - return INV_ERROR_FILE_READ; - }*/ - - result = inv_load_mpl_states(calData, len); - return result; -} - -/** - * @brief Loads a type 1 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature, - * - gyro biases for X, Y, Z axes, - * - accel biases for X, Y, Z axes. - * This calibration data would normally be produced by the MPU Self - * Test and its size is 36 bytes (header and checksum included). - * Calibration format type 1 is produced by the MPU Self Test and - * substitutes the type 0 : inv_load_cal_V0(). - * - * @pre - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) -{ - return INV_SUCCESS; -} - -/** - * @brief Loads a set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * - * @pre - * - * - * @param calData - * A pointer to an array of bytes to be parsed. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal(unsigned char *calData) -{ - int calType = 0; - int len = 0; - //int ptr; - //uint32_t chk = 0; - //uint32_t cmp_chk = 0; - - /*load_func_t loaders[] = { - inv_load_cal_V0, - inv_load_cal_V1, - }; - */ - - inv_load_cal_V0(calData, len); - - /* read the header (type and len) - len is the total record length including header and checksum */ - len = 0; - len += 16777216L * ((int)calData[0]); - len += 65536L * ((int)calData[1]); - len += 256 * ((int)calData[2]); - len += (int)calData[3]; - - calType = ((int)calData[4]) * 256 + ((int)calData[5]); - if (calType > 5) { - MPL_LOGE("Unsupported calibration file format %d. " - "Valid types 0..5\n", calType); - return INV_ERROR_INVALID_PARAMETER; - } - - /* call the proper method to read in the data */ - //return loaders[calType] (calData, len); - return 0; -} - -/** - * @brief Stores a set of calibration data. - * It generates a binary data set containing calibration data. - * The binary data set is intended to be stored into a file. - * - * @pre inv_dmp_open() - * - * @param calData - * A pointer to an array of bytes to be stored. - * @param length - * The amount of bytes available in the array. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_store_cal(unsigned char *calData, size_t length) -{ - inv_error_t res = 0; - size_t size; - - STORECAL_LOG("Entering inv_store_cal\n"); - - inv_get_mpl_state_size(&size); - - MPL_LOGI("inv_get_mpl_state_size() : size=%d", size); - - /* store data */ - res = inv_save_mpl_states(calData, size); - if(res != 0) - { - MPL_LOGE("inv_save_mpl_states() failed"); - } - - STORECAL_LOG("Exiting inv_store_cal\n"); - return INV_SUCCESS; -} - -/** - * @brief Load a calibration file. - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return 0 or error code. - */ -inv_error_t inv_load_calibration(void) -{ - unsigned char *calData= NULL; - inv_error_t result = 0; - size_t bytesRead = 0; - - result = inv_read_cal(&calData, &bytesRead); - if(result != INV_SUCCESS) { - MPL_LOGE("Could not load cal file - " - "aborting\n"); - goto free_mem_n_exit; - } - - result = inv_load_mpl_states(calData, bytesRead); - if (result != INV_SUCCESS) { - MPL_LOGE("Could not load the calibration data - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - } - -free_mem_n_exit: - inv_free(calData); - return result; -} - -/** - * @brief Store runtime calibration data to a file - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return 0 or error code. - */ -inv_error_t inv_store_calibration(void) -{ - unsigned char *calData; - inv_error_t result; - size_t length; - - result = inv_get_mpl_state_size(&length); - calData = (unsigned char *)inv_malloc(length); - if (!calData) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", length); - return INV_ERROR_MEMORY_EXAUSTED; - } - else { - MPL_LOGI("mpl state size = %d", length); - } - - result = inv_save_mpl_states(calData, length); - if (result != INV_SUCCESS) { - MPL_LOGE("Could not save mpl states - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - } - else { - MPL_LOGE("calData from inv_save_mpl_states, size=%d", - strlen((char *)calData)); - } - - result = inv_write_cal(calData, length); - if (result != INV_SUCCESS) { - MPL_LOGE("Could not store calibrated data on file - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - - } - -free_mem_n_exit: - inv_free(calData); - return result; -} - -/** - * @} - */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h deleted file mode 100644 index 115b34c..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $ - * - ******************************************************************************/ - -#ifndef INV_MPL_STORED_DATA_H -#define INV_MPL_STORED_DATA_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Includes. -*/ -#include "mltypes.h" - -/* - Defines -*/ -#define MLCAL_FILE "/data/inv_cal_data.bin" - -/* - APIs -*/ -inv_error_t inv_load_calibration(void); -inv_error_t inv_store_calibration(void); - -/* - Internal APIs -*/ -inv_error_t inv_read_cal(unsigned char **, size_t *); -inv_error_t inv_write_cal(unsigned char *cal, size_t len); -inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len); -inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len); - -/* - Other prototypes -*/ -inv_error_t inv_load_cal(unsigned char *calData); -inv_error_t inv_store_cal(unsigned char *calData, size_t length); - -#ifdef __cplusplus -} -#endif -#endif /* INV_MPL_STORED_DATA_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c deleted file mode 100644 index d0c4513..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c +++ /dev/null @@ -1,423 +0,0 @@ -#include <string.h> -#include <stdio.h> -#include "ml_sysfs_helper.h" -#include <dirent.h> -#include <ctype.h> -#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" - -enum PROC_SYSFS_CMD { - CMD_GET_SYSFS_PATH, - CMD_GET_DMP_PATH, - CMD_GET_CHIP_NAME, - CMD_GET_SYSFS_KEY, - CMD_GET_TRIGGER_PATH, - CMD_GET_DEVICE_NODE -}; -static char sysfs_path[100]; -static char *chip_name[] = { - "ITG3500", - "MPU6050", - "MPU9150", - "MPU3050", - "MPU6500" -}; -static int chip_ind; -static int initialized =0; -static int status = 0; -static int iio_initialized = 0; -static int iio_dev_num = 0; - -#define IIO_MAX_NAME_LENGTH 30 - -#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" -#define FORMAT_TYPE_FILE "%s_type" - -#define CHIP_NUM ARRAY_SIZE(chip_name) - -static const char *iio_dir = "/sys/bus/iio/devices/"; - -/** - * find_type_by_name() - function to match top level types by name - * @name: top level type instance name - * @type: the type of top level instance being sort - * - * Typical types this is used for are device and trigger. - **/ -int find_type_by_name(const char *name, const char *type) -{ - const struct dirent *ent; - int number, numstrlen; - - FILE *nameFile; - DIR *dp; - char thisname[IIO_MAX_NAME_LENGTH]; - char *filename; - - dp = opendir(iio_dir); - if (dp == NULL) { - printf("No industrialio devices available"); - return -ENODEV; - } - - while (ent = readdir(dp), ent != NULL) { - if (strcmp(ent->d_name, ".") != 0 && - strcmp(ent->d_name, "..") != 0 && - strlen(ent->d_name) > strlen(type) && - strncmp(ent->d_name, type, strlen(type)) == 0) { - numstrlen = sscanf(ent->d_name + strlen(type), - "%d", - &number); - /* verify the next character is not a colon */ - if (strncmp(ent->d_name + strlen(type) + numstrlen, - ":", - 1) != 0) { - filename = malloc(strlen(iio_dir) - + strlen(type) - + numstrlen - + 6); - if (filename == NULL) - return -ENOMEM; - sprintf(filename, "%s%s%d/name", - iio_dir, - type, - number); - nameFile = fopen(filename, "r"); - if (!nameFile) - continue; - free(filename); - fscanf(nameFile, "%s", thisname); - if (strcmp(name, thisname) == 0) - return number; - fclose(nameFile); - } - } - } - return -ENODEV; -} - -/* mode 0: search for which chip in the system and fill sysfs path - mode 1: return event number - */ -static int parsing_proc_input(int mode, char *name){ - const char input[] = "/proc/bus/input/devices"; - char line[4096], d; - char tmp[100]; - FILE *fp; - int i, j, result, find_flag; - int event_number = -1; - int input_number = -1; - - if(NULL == (fp = fopen(input, "rt")) ){ - return -1; - } - result = 1; - find_flag = 0; - while(result != 0 && find_flag < 2){ - i = 0; - d = 0; - memset(line, 0, 100); - while(d != '\n'){ - result = fread(&d, 1, 1, fp); - if(result == 0){ - line[0] = 0; - break; - } - sprintf(&line[i], "%c", d); - i ++; - } - if(line[0] == 'N'){ - i = 1; - while(line[i] != '"'){ - i++; - } - i++; - j = 0; - find_flag = 0; - if (mode == 0){ - while(j < CHIP_NUM){ - if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){ - find_flag = 1; - chip_ind = j; - } - j++; - } - } else if (mode != 0){ - if(!memcmp(&line[i], name, strlen(name))){ - find_flag = 1; - } - } - } - if(find_flag){ - if(mode == 0){ - if(line[0] == 'S'){ - memset(tmp, 0, 100); - i =1; - while(line[i] != '=') i++; - i++; - j = 0; - while(line[i] != '\n'){ - tmp[j] = line[i]; - i ++; j++; - } - sprintf(sysfs_path, "%s%s", "/sys", tmp); - find_flag++; - } - } else if(mode == 1){ - if(line[0] == 'H') { - i = 2; - while(line[i] != '=') i++; - while(line[i] != 't') i++; - i++; - event_number = 0; - while(line[i] != '\n'){ - if(line[i] >= '0' && line[i] <= '9') - event_number = event_number*10 + line[i]-0x30; - i ++; - } - find_flag ++; - } - } else if (mode == 2) { - if(line[0] == 'S'){ - memset(tmp, 0, 100); - i =1; - while(line[i] != '=') i++; - i++; - j = 0; - while(line[i] != '\n'){ - tmp[j] = line[i]; - i ++; j++; - } - input_number = 0; - if(tmp[j-2] >= '0' && tmp[j-2] <= '9') - input_number += (tmp[j-2]-0x30)*10; - if(tmp[j-1] >= '0' && tmp[j-1] <= '9') - input_number += (tmp[j-1]-0x30); - find_flag++; - } - } - } - } - fclose(fp); - if(find_flag == 0){ - return -1; - } - if(0 == mode) - status = 1; - if (mode == 1) - return event_number; - if (mode == 2) - return input_number; - return 0; - -} -static void init_iio() { - int i, j; - char iio_chip[10]; - int dev_num; - for(j=0; j< CHIP_NUM; j++) { - for (i=0; i<strlen(chip_name[j]); i++) { - iio_chip[i] = tolower(chip_name[j][i]); - } - iio_chip[strlen(chip_name[0])] = '\0'; - dev_num = find_type_by_name(iio_chip, "iio:device"); - if(dev_num >= 0) { - iio_initialized = 1; - iio_dev_num = dev_num; - chip_ind = j; - } - } -} - -static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data) -{ - char key_path[100]; - FILE *fp; - int i, result; - if(initialized == 0){ - parsing_proc_input(0, NULL); - initialized = 1; - } - if(initialized && status == 0) { - init_iio(); - if (iio_initialized == 0) - return -1; - } - - memset(key_path, 0, 100); - switch(cmd){ - case CMD_GET_SYSFS_PATH: - if (iio_initialized == 1) - sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num); - else - sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu"); - break; - case CMD_GET_DMP_PATH: - if (iio_initialized == 1) - sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num); - else - sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware"); - break; - case CMD_GET_CHIP_NAME: - sprintf(data, "%s", chip_name[chip_ind]); - break; - case CMD_GET_TRIGGER_PATH: - sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num); - break; - case CMD_GET_DEVICE_NODE: - sprintf(data, "/dev/iio:device%d", iio_dev_num); - break; - case CMD_GET_SYSFS_KEY: - memset(key_path, 0, 100); - if (iio_initialized == 1) - sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num); - else - sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key"); - - if((fp = fopen(key_path, "rt")) == NULL) - return -1; - for(i=0;i<16;i++){ - fscanf(fp, "%02x", &result); - data[i] = (char)result; - } - - fclose(fp); - break; - default: - break; - } - return 0; -} -/** - * @brief return sysfs key. if the key is not available - * return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the key - * It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_sysfs_key(unsigned char *key) -{ - if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -/** - * @brief return the sysfs path. If the path is not - * found yet. return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the sysfs - * path. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_sysfs_path(char *name) -{ - if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -inv_error_t inv_get_sysfs_abs_path(char *name) -{ - strcpy(name, MPU_SYSFS_ABS_PATH); - return INV_SUCCESS; -} - -/** - * @brief return the dmp file path. If the path is not - * found yet. return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the dmp file - * path. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_dmpfile(char *name) -{ - if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} -/** - * @brief return the chip name. If the chip is not - * found yet. return false. So the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the chip name - * path(8 bytes). It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_chip_name(char *name) -{ - if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} -/** - * @brief return event handler number. If the handler number is not found - * return false. the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the chip name - * path(8 bytes). It should be zeroed before calling this function. - * Or it could have unpredicable result. - * @int *num: event number store - */ -inv_error_t inv_get_handler_number(const char *name, int *num) -{ - initialized = 0; - if ((*num = parsing_proc_input(1, (char *)name)) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -/** - * @brief return input number. If the handler number is not found - * return false. the return value must be checked - * to make sure the path is valid. - * @unsigned char *name: This should be array big enough to hold the chip name - * path(8 bytes). It should be zeroed before calling this function. - * Or it could have unpredicable result. - * @int *num: input number store - */ -inv_error_t inv_get_input_number(const char *name, int *num) -{ - initialized = 0; - if ((*num = parsing_proc_input(2, (char *)name)) < 0) - return INV_ERROR_NOT_OPENED; - else { - return INV_SUCCESS; - } -} - -/** - * @brief return iio trigger name. If iio is not initialized, return false. - * So the return must be checked to make sure the numeber is valid. - * @unsigned char *name: This should be array big enough to hold the trigger - * name. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_iio_trigger_path(char *name) -{ - if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} - -/** - * @brief return iio device node. If iio is not initialized, return false. - * So the return must be checked to make sure the numeber is valid. - * @unsigned char *name: This should be array big enough to hold the device - * node. It should be zeroed before calling this function. - * Or it could have unpredicable result. - */ -inv_error_t inv_get_iio_device_node(char *name) -{ - if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0) - return INV_ERROR_NOT_OPENED; - else - return INV_SUCCESS; -} diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h deleted file mode 100644 index 9470874..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id$ - * - ******************************************************************************/ - -#ifndef MLDMP_SYSFS_HELPER_H__ -#define MLDMP_SYSFS_HELPER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#include "invensense.h" - -int find_type_by_name(const char *name, const char *type); -inv_error_t inv_get_sysfs_path(char *name); -inv_error_t inv_get_sysfs_abs_path(char *name); -inv_error_t inv_get_dmpfile(char *name); -inv_error_t inv_get_chip_name(char *name); -inv_error_t inv_get_sysfs_key(unsigned char *key); -inv_error_t inv_get_handler_number(const char *name, int *num); -inv_error_t inv_get_input_number(const char *name, int *num); -inv_error_t inv_get_iio_trigger_path(char *name); -inv_error_t inv_get_iio_device_node(char *name); - -#ifdef __cplusplus -} -#endif -#endif /* MLDMP_SYSFS_HELPER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h deleted file mode 100644 index d4f8912..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h +++ /dev/null @@ -1,99 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef _MLOS_H -#define _MLOS_H - -#ifndef __KERNEL__ -#include <stdio.h> -#endif -#include <pthread.h> - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(LINUX) || defined(__KERNEL__) -typedef pthread_mutex_t* HANDLE; -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /* - MLOSCreateFile defines. - */ - -#define MLOS_GENERIC_READ ((unsigned int)0x80000000) -#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) -#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) -#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) -#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) - - /* ---------- */ - /* - Enums. - */ - /* ---------- */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - -#ifndef __KERNEL__ -#include <string.h> - void *inv_malloc(unsigned int numBytes); - inv_error_t inv_free(void *ptr); - inv_error_t inv_create_mutex(HANDLE *mutex); - inv_error_t inv_lock_mutex(HANDLE mutex); - inv_error_t inv_unlock_mutex(HANDLE mutex); - FILE *inv_fopen(char *filename); - void inv_fclose(FILE *fp); - - inv_error_t inv_destroy_mutex(HANDLE handle); - - void inv_sleep(int mSecs); - unsigned long inv_get_tick_count(void); - - /* Kernel implmentations */ -#define GFP_KERNEL (0x70) - static inline void *kmalloc(size_t size, - unsigned int gfp_flags) - { - (void)gfp_flags; - return inv_malloc((unsigned int)size); - } - static inline void *kzalloc(size_t size, unsigned int gfp_flags) - { - void *tmp = inv_malloc((unsigned int)size); - (void)gfp_flags; - if (tmp) - memset(tmp, 0, size); - return tmp; - } - static inline void kfree(void *ptr) - { - inv_free(ptr); - } - static inline void msleep(long msecs) - { - inv_sleep(msecs); - } - static inline void udelay(unsigned long usecs) - { - inv_sleep((usecs + 999) / 1000); - } -#else -#include <linux/delay.h> -#endif - -#ifdef __cplusplus -} -#endif -#endif /* _MLOS_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c deleted file mode 100644 index 5424508..0000000 --- a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c +++ /dev/null @@ -1,190 +0,0 @@ -/* - $License: - Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - ******************************************************************************/ - -/** - * @defgroup MLOS - * @brief OS Interface. - * - * @{ - * @file mlos.c - * @brief OS Interface. - */ - -/* ------------- */ -/* - Includes. - */ -/* ------------- */ - -#include <sys/time.h> -#include <unistd.h> -#include <pthread.h> -#include <stdlib.h> -#include <errno.h> - -#include "stdint_invensense.h" -#include "mlos.h" - - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Allocate space - * @param num_bytes number of bytes - * @return pointer to allocated space - */ -void *inv_malloc(unsigned int num_bytes) -{ - // Allocate space. - void *alloc_ptr = malloc(num_bytes); - return alloc_ptr; -} - - -/** - * @brief Free allocated space - * @param ptr pointer to space to deallocate - * @return error code. - */ -inv_error_t inv_free(void *ptr) -{ - if (ptr) - free(ptr); - return INV_SUCCESS; -} - - -/** - * @brief Mutex create function - * @param mutex pointer to mutex handle - * @return error code. - */ -inv_error_t inv_create_mutex(HANDLE *mutex) -{ - int res; - pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); - if(pm == NULL) - return INV_ERROR; - - res = pthread_mutex_init(pm, NULL); - if(res == -1) { - free(pm); - return INV_ERROR_OS_CREATE_FAILED; - } - - *mutex = (HANDLE)pm; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex lock function - * @param mutex Mutex handle - * @return error code. - */ -inv_error_t inv_lock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t *)mutex; - - res = pthread_mutex_lock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex unlock function - * @param mutex mutex handle - * @return error code. - */ -inv_error_t inv_unlock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t *)mutex; - - res = pthread_mutex_unlock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief open file - * @param filename name of the file to open. - * @return error code. - */ -FILE *inv_fopen(char *filename) -{ - FILE *fp = fopen(filename, "r"); - return fp; -} - - -/** - * @brief close the file. - * @param fp handle to file to close. - * @return error code. - */ -void inv_fclose(FILE *fp) -{ - fclose(fp); -} - -/** - * @brief Close Handle - * @param handle handle to the resource. - * @return Zero if success, an error code otherwise. - */ -inv_error_t inv_destroy_mutex(HANDLE handle) -{ - int error; - pthread_mutex_t *pm = (pthread_mutex_t *)handle; - error = pthread_mutex_destroy(pm); - if (error) - return errno; - free((void*) handle); - - return INV_SUCCESS;} - - -/** - * @brief Sleep function. - */ -void inv_sleep(int m_secs) -{ - usleep(m_secs * 1000); -} - - -/** - * @brief get system's internal tick count. - * Used for time reference. - * @return current tick count. - */ -unsigned long inv_get_tick_count() -{ - struct timeval tv; - - if (gettimeofday(&tv, NULL) != 0) - return 0; - - return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); -} - -/** @} */ - diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c deleted file mode 100644 index 8317957..0000000 --- a/60xx/libsensors_iio/software/core/mllite/message_layer.c +++ /dev/null @@ -1,59 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup Message_Layer message_layer
- * @brief Motion Library - Message Layer
- * Holds Low Occurance messages
- *
- * @{
- * @file message_layer.c
- * @brief Holds Low Occurance Messages.
- */
-#include "message_layer.h"
-#include "log.h"
-
-struct message_holder_t {
- long message;
-};
-
-static struct message_holder_t mh;
-
-/** Sets a message.
-* @param[in] set The flags to set.
-* @param[in] clear Before setting anything this will clear these messages,
-* which is useful for mutually exclusive messages such
-* a motion or no motion message.
-* @param[in] level Level of the messages. It starts at 0, and may increase
-* in the future to allow more messages if the bit storage runs out.
-*/
-void inv_set_message(long set, long clear, int level)
-{
- if (level == 0) {
- mh.message &= ~clear;
- mh.message |= set;
- }
-}
-
-/** Returns Message Flags for Level 0 Messages.
-* Levels are to allow expansion of more messages in the future.
-* @param[in] clear If set, will clear the message. Typically this will be set
-* for one reader, so that you don't get the same message over and over.
-* @return bit field to corresponding message.
-*/
-long inv_get_message_level_0(int clear)
-{
- long msg;
- msg = mh.message;
- if (clear) {
- mh.message = 0;
- }
- return msg;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h deleted file mode 100644 index df0c0e2..0000000 --- a/60xx/libsensors_iio/software/core/mllite/message_layer.h +++ /dev/null @@ -1,35 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_MESSAGE_LAYER_H__
-#define INV_MESSAGE_LAYER_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* Level 0 Type Messages */
- /** A motion event has occured */
-#define INV_MSG_MOTION_EVENT (0x01)
- /** A no motion event has occured */
-#define INV_MSG_NO_MOTION_EVENT (0x02)
- /** A setting of the gyro bias has occured */
-#define INV_MSG_NEW_GB_EVENT (0x04)
- /** A setting of the compass bias has occured */
-#define INV_MSG_NEW_CB_EVENT (0x08)
- /** A setting of the accel bias has occured */
-#define INV_MSG_NEW_AB_EVENT (0x10)
-
- void inv_set_message(long set, long clear, int level);
- long inv_get_message_level_0(int clear);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MESSAGE_LAYER_H__
diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.c b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c deleted file mode 100644 index c30d693..0000000 --- a/60xx/libsensors_iio/software/core/mllite/ml_math_func.c +++ /dev/null @@ -1,706 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -/** - * @defgroup ML_MATH_FUNC ml_math_func - * @brief Motion Library - Math Functions - * Common math functions the Motion Library - * - * @{ - * @file ml_math_func.c - * @brief Math Functions. - */ - -#include "mlmath.h" -#include "ml_math_func.h" -#include "mlinclude.h" -#include <string.h> - -/** @internal - * Does the cross product of compass by gravity, then converts that - * to the world frame using the quaternion, then computes the angle that - * is made. - * - * @param[in] compass Compass Vector (Body Frame), length 3 - * @param[in] grav Gravity Vector (Body Frame), length 3 - * @param[in] quat Quaternion, Length 4 - * @return Angle Cross Product makes after quaternion rotation. - */ -float inv_compass_angle(const long *compass, const long *grav, const float *quat) -{ - float cgcross[4], q1[4], q2[4], qi[4]; - float angW; - - // Compass cross Gravity - cgcross[0] = 0.f; - cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; - cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; - cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; - - // Now convert cross product into world frame - inv_q_multf(quat, cgcross, q1); - inv_q_invertf(quat, qi); - inv_q_multf(q1, qi, q2); - - // Protect against atan2 of 0,0 - if ((q2[2] == 0.f) && (q2[1] == 0.f)) - return 0.f; - - // This is the unfiltered heading correction - angW = -atan2f(q2[2], q2[1]); - return angW; -} - -/** - * @brief The gyro data magnitude squared : - * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. - * @param[in] gyro Gyro data scaled with 1 dps = 2^16 - * @return the computed magnitude squared output of the gyroscope. - */ -unsigned long inv_get_gyro_sum_of_sqr(const long *gyro) -{ - unsigned long gmag = 0; - long temp; - int kk; - - for (kk = 0; kk < 3; ++kk) { - temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); - gmag += temp * temp; - } - - return gmag; -} - -/** Performs a multiply and shift by 29. These are good functions to write in assembly on - * with devices with small memory where you want to get rid of the long long which some - * assemblers don't handle well - * @param[in] a - * @param[in] b - * @return ((long long)a*b)>>29 -*/ -long inv_q29_mult(long a, long b) -{ -#ifdef UMPL_ELIMINATE_64BIT - long result; - result = (long)((float)a * b / (1L << 29)); - return result; -#else - long long temp; - long result; - temp = (long long)a * b; - result = (long)(temp >> 29); - return result; -#endif -} - -/** Performs a multiply and shift by 30. These are good functions to write in assembly on - * with devices with small memory where you want to get rid of the long long which some - * assemblers don't handle well - * @param[in] a - * @param[in] b - * @return ((long long)a*b)>>30 -*/ -long inv_q30_mult(long a, long b) -{ -#ifdef UMPL_ELIMINATE_64BIT - long result; - result = (long)((float)a * b / (1L << 30)); - return result; -#else - long long temp; - long result; - temp = (long long)a * b; - result = (long)(temp >> 30); - return result; -#endif -} - -#ifndef UMPL_ELIMINATE_64BIT -long inv_q30_div(long a, long b) -{ - long long temp; - long result; - temp = (((long long)a) << 30) / b; - result = (long)temp; - return result; -} -#endif - -/** Performs a multiply and shift by shift. These are good functions to write - * in assembly on with devices with small memory where you want to get rid of - * the long long which some assemblers don't handle well - * @param[in] a First multicand - * @param[in] b Second multicand - * @param[in] shift Shift amount after multiplying - * @return ((long long)a*b)<<shift -*/ -#ifndef UMPL_ELIMINATE_64BIT -long inv_q_shift_mult(long a, long b, int shift) -{ - long result; - result = (long)(((long long)a * b) >> shift); - return result; -} -#endif - -/** Performs a fixed point quaternion multiply. -* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled -* to 2^30 -* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled -* to 2^30 -* @param[out] qProd Product after quaternion multiply. Length 4. -* 1.0 scaled to 2^30. -*/ -void inv_q_mult(const long *q1, const long *q2, long *qProd) -{ - INVENSENSE_FUNC_START; - qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) - - inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]); - - qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) + - inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]); - - qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) + - inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]); - - qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) - - inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]); -} - -/** Performs a fixed point quaternion addition. -* @param[in] q1 First Quaternion term, length 4. 1.0 scaled -* to 2^30 -* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled -* to 2^30 -* @param[out] qSum Sum after quaternion summation. Length 4. -* 1.0 scaled to 2^30. -*/ -void inv_q_add(long *q1, long *q2, long *qSum) -{ - INVENSENSE_FUNC_START; - qSum[0] = q1[0] + q2[0]; - qSum[1] = q1[1] + q2[1]; - qSum[2] = q1[2] + q2[2]; - qSum[3] = q1[3] + q2[3]; -} - -void inv_vector_normalize(long *vec, int length) -{ - INVENSENSE_FUNC_START; - double normSF = 0; - int ii; - for (ii = 0; ii < length; ii++) { - normSF += - inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]); - } - if (normSF > 0) { - normSF = 1 / sqrt(normSF); - for (ii = 0; ii < length; ii++) { - vec[ii] = (int)((double)vec[ii] * normSF); - } - } else { - vec[0] = 1073741824L; - for (ii = 1; ii < length; ii++) { - vec[ii] = 0; - } - } -} - -void inv_q_normalize(long *q) -{ - INVENSENSE_FUNC_START; - inv_vector_normalize(q, 4); -} - -void inv_q_invert(const long *q, long *qInverted) -{ - INVENSENSE_FUNC_START; - qInverted[0] = q[0]; - qInverted[1] = -q[1]; - qInverted[2] = -q[2]; - qInverted[3] = -q[3]; -} - -double quaternion_to_rotation_angle(const long *quat) { - double quat0 = (double )quat[0] / 1073741824; - if (quat0 > 1.0f) { - quat0 = 1.0; - } else if (quat0 < -1.0f) { - quat0 = -1.0; - } - - return acos(quat0)*2*180/M_PI; -} - -/** Rotates a 3-element vector by Rotation defined by Q -*/ -void inv_q_rotate(const long *q, const long *in, long *out) -{ - long q_temp1[4], q_temp2[4]; - long in4[4], out4[4]; - - // Fixme optimize - in4[0] = 0; - memcpy(&in4[1], in, 3 * sizeof(long)); - inv_q_mult(q, in4, q_temp1); - inv_q_invert(q, q_temp2); - inv_q_mult(q_temp1, q_temp2, out4); - memcpy(out, &out4[1], 3 * sizeof(long)); -} - -void inv_q_multf(const float *q1, const float *q2, float *qProd) -{ - INVENSENSE_FUNC_START; - qProd[0] = - (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); - qProd[1] = - (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); - qProd[2] = - (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); - qProd[3] = - (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); -} - -void inv_q_addf(const float *q1, const float *q2, float *qSum) -{ - INVENSENSE_FUNC_START; - qSum[0] = q1[0] + q2[0]; - qSum[1] = q1[1] + q2[1]; - qSum[2] = q1[2] + q2[2]; - qSum[3] = q1[3] + q2[3]; -} - -void inv_q_normalizef(float *q) -{ - INVENSENSE_FUNC_START; - float normSF = 0; - float xHalf = 0; - normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - if (normSF < 2) { - xHalf = 0.5f * normSF; - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - q[0] *= normSF; - q[1] *= normSF; - q[2] *= normSF; - q[3] *= normSF; - } else { - q[0] = 1.0; - q[1] = 0.0; - q[2] = 0.0; - q[3] = 0.0; - } - normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); -} - -/** Performs a length 4 vector normalization with a square root. -* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero. -*/ -void inv_q_norm4(float *q) -{ - float mag; - mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - if (mag) { - q[0] /= mag; - q[1] /= mag; - q[2] /= mag; - q[3] /= mag; - } else { - q[0] = 1.f; - q[1] = 0.f; - q[2] = 0.f; - q[3] = 0.f; - } -} - -void inv_q_invertf(const float *q, float *qInverted) -{ - INVENSENSE_FUNC_START; - qInverted[0] = q[0]; - qInverted[1] = -q[1]; - qInverted[2] = -q[2]; - qInverted[3] = -q[3]; -} - -/** - * Converts a quaternion to a rotation matrix. - * @param[in] quat 4-element quaternion in fixed point. One is 2^30. - * @param[out] rot Rotation matrix in fixed point. One is 2^30. The - * First 3 elements of the rotation matrix, represent - * the first row of the matrix. Rotation matrix multiplied - * by a 3 element column vector transform a vector from Body - * to World. - */ -void inv_quaternion_to_rotation(const long *quat, long *rot) -{ - rot[0] = - inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], - quat[0]) - - 1073741824L; - rot[1] = - inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); - rot[2] = - inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); - rot[3] = - inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); - rot[4] = - inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], - quat[0]) - - 1073741824L; - rot[5] = - inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); - rot[6] = - inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); - rot[7] = - inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); - rot[8] = - inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], - quat[0]) - - 1073741824L; -} - -/** - * Converts a quaternion to a rotation vector. A rotation vector is - * a method to represent a 4-element quaternion vector in 3-elements. - * To get the quaternion from the 3-elements, The last 3-elements of - * the quaternion will be the given rotation vector. The first element - * of the quaternion will be the positive value that will be required - * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units. - * @param[in] quat 4-element quaternion in fixed point. One is 2^30. - * @param[out] rot Rotation vector in fixed point. One is 2^30. - */ -void inv_quaternion_to_rotation_vector(const long *quat, long *rot) -{ - rot[0] = quat[1]; - rot[1] = quat[2]; - rot[2] = quat[3]; - - if (quat[0] < 0.0) { - rot[0] = -rot[0]; - rot[1] = -rot[1]; - rot[2] = -rot[2]; - } -} - -/** Converts a 32-bit long to a big endian byte stream */ -unsigned char *inv_int32_to_big8(long x, unsigned char *big8) -{ - big8[0] = (unsigned char)((x >> 24) & 0xff); - big8[1] = (unsigned char)((x >> 16) & 0xff); - big8[2] = (unsigned char)((x >> 8) & 0xff); - big8[3] = (unsigned char)(x & 0xff); - return big8; -} - -/** Converts a big endian byte stream into a 32-bit long */ -long inv_big8_to_int32(const unsigned char *big8) -{ - long x; - x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) - | ((long)big8[3]); - return x; -} - -/** Converts a big endian byte stream into a 16-bit integer (short) */ -short inv_big8_to_int16(const unsigned char *big8) -{ - short x; - x = ((short)big8[0] << 8) | ((short)big8[1]); - return x; -} - -/** Converts a little endian byte stream into a 16-bit integer (short) */ -short inv_little8_to_int16(const unsigned char *little8) -{ - short x; - x = ((short)little8[1] << 8) | ((short)little8[0]); - return x; -} - -/** Converts a 16-bit short to a big endian byte stream */ -unsigned char *inv_int16_to_big8(short x, unsigned char *big8) -{ - big8[0] = (unsigned char)((x >> 8) & 0xff); - big8[1] = (unsigned char)(x & 0xff); - return big8; -} - -void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) -{ - int k, l, i, j; - for (i = 0, k = 0; i < *n; i++, k++) { - for (j = 0, l = 0; j < *n; j++, l++) { - if (i == x) - i++; - if (j == y) - j++; - *(b + 6 * k + l) = *(a + 6 * i + j); - } - } - *n = *n - 1; -} - -void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) -{ - int k, l, i, j; - for (i = 0, k = 0; i < *n; i++, k++) { - for (j = 0, l = 0; j < *n; j++, l++) { - if (i == x) - i++; - if (j == y) - j++; - *(b + 6 * k + l) = *(a + 6 * i + j); - } - } - *n = *n - 1; -} - -float inv_matrix_det(float *p, int *n) -{ - float d[6][6], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 7) - *(p + 1) ** (p + 6)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_inc(p, &d[0][0], n, i, j); - sum = - sum + *(p + 6 * i + j) * SIGNM(i + - j) * - inv_matrix_det(&d[0][0], n); - } - - return (sum); -} - -double inv_matrix_detd(double *p, int *n) -{ - double d[6][6], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 7) - *(p + 1) ** (p + 6)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_incd(p, &d[0][0], n, i, j); - sum = - sum + *(p + 6 * i + j) * SIGNM(i + - j) * - inv_matrix_detd(&d[0][0], n); - } - - return (sum); -} - -/** Wraps angle from (-M_PI,M_PI] - * @param[in] ang Angle in radians to wrap - * @return Wrapped angle from (-M_PI,M_PI] - */ -float inv_wrap_angle(float ang) -{ - if (ang > M_PI) - return ang - 2 * (float)M_PI; - else if (ang <= -(float)M_PI) - return ang + 2 * (float)M_PI; - else - return ang; -} - -/** Finds the minimum angle difference ang1-ang2 such that difference - * is between [-M_PI,M_PI] - * @param[in] ang1 - * @param[in] ang2 - * @return angle difference ang1-ang2 - */ -float inv_angle_diff(float ang1, float ang2) -{ - float d; - ang1 = inv_wrap_angle(ang1); - ang2 = inv_wrap_angle(ang2); - d = ang1 - ang2; - if (d > M_PI) - d -= 2 * (float)M_PI; - else if (d < -(float)M_PI) - d += 2 * (float)M_PI; - return d; -} - -/** bernstein hash, derived from public domain source */ -uint32_t inv_checksum(const unsigned char *str, int len) -{ - uint32_t hash = 5381; - int i, c; - - for (i = 0; i < len; i++) { - c = *(str + i); - hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ - } - - return hash; -} - -static unsigned short inv_row_2_scale(const signed char *row) -{ - unsigned short b; - - if (row[0] > 0) - b = 0; - else if (row[0] < 0) - b = 4; - else if (row[1] > 0) - b = 1; - else if (row[1] < 0) - b = 5; - else if (row[2] > 0) - b = 2; - else if (row[2] < 0) - b = 6; - else - b = 7; // error - return b; -} - - -/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. -* @param[in] mtx Orientation matrix to convert to a scalar. -* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the -* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent -* the column the one is on for the second row with bit number 5 being the sign. -* The next 2 bits (6 and 7) represent the column the one is on for the third row with -* bit number 8 being the sign. In binary the identity matrix would therefor be: -* 010_001_000 or 0x88 in hex. -*/ -unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) -{ - - unsigned short scalar; - - /* - XYZ 010_001_000 Identity Matrix - XZY 001_010_000 - YXZ 010_000_001 - YZX 000_010_001 - ZXY 001_000_010 - ZYX 000_001_010 - */ - - scalar = inv_row_2_scale(mtx); - scalar |= inv_row_2_scale(mtx + 3) << 3; - scalar |= inv_row_2_scale(mtx + 6) << 6; - - - return scalar; -} - -/** Uses the scalar orientation value to convert from chip frame to body frame -* @param[in] orientation A scalar that represent how to go from chip to body frame -* @param[in] input Input vector, length 3 -* @param[out] output Output vector, length 3 -*/ -void inv_convert_to_body(unsigned short orientation, const long *input, long *output) -{ - output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004); - output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020); - output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100); -} - -/** Uses the scalar orientation value to convert from body frame to chip frame -* @param[in] orientation A scalar that represent how to go from chip to body frame -* @param[in] input Input vector, length 3 -* @param[out] output Output vector, length 3 -*/ -void inv_convert_to_chip(unsigned short orientation, const long *input, long *output) -{ - output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004); - output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020); - output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100); -} - - -/** Uses the scalar orientation value to convert from chip frame to body frame and -* apply appropriate scaling. -* @param[in] orientation A scalar that represent how to go from chip to body frame -* @param[in] sensitivity Sensitivity scale -* @param[in] input Input vector, length 3 -* @param[out] output Output vector, length 3 -*/ -void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output) -{ - output[0] = inv_q30_mult(input[orientation & 0x03] * - SIGNSET(orientation & 0x004), sensitivity); - output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] * - SIGNSET(orientation & 0x020), sensitivity); - output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] * - SIGNSET(orientation & 0x100), sensitivity); -} - -/** find a norm for a vector -* @param[in] x a vector [3x1] -* @return the normalize vector. -*/ -double inv_vector_norm(const float *x) -{ - return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); -} - -void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { - int i; - // initial state to zero - pFilter->state[0] = 0; - pFilter->state[1] = 0; - - // set up coefficients - for (i=0; i<5; i++) { - pFilter->c[i] = pBiquadCoeff[i]; - } -} - -void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) -{ - pFilter->input = input; - pFilter->output = input; - pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); - pFilter->state[1] = pFilter->state[0]; -} - -float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { - float stateZero; - - pFilter->input = input; - // calculate the new state; - stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] - - pFilter->c[3]*pFilter->state[1]; - - pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] - + pFilter->c[1]*pFilter->state[1]; - - // update the output and state - pFilter->output = pFilter->output * pFilter->c[4]; - pFilter->state[1] = pFilter->state[0]; - pFilter->state[0] = stateZero; - return pFilter->output; -} - -void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { - - cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; - cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; - cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; -} - -/** - * @} - */ diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h deleted file mode 100644 index 535d849..0000000 --- a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#ifndef INVENSENSE_INV_MATH_FUNC_H__ -#define INVENSENSE_INV_MATH_FUNC_H__ - -#include "mltypes.h" - -#define GYRO_MAG_SQR_SHIFT 6 -#define NUM_ROTATION_MATRIX_ELEMENTS (9) -#define ROT_MATRIX_SCALE_LONG (1073741824L) -#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) -#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ - ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) -#define SIGNM(k)((int)(k)&1?-1:1) -#define SIGNSET(x) ((x) ? -1 : +1) - -#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f - -#ifdef __cplusplus -extern "C" { -#endif - - typedef struct { - float state[4]; - float c[5]; - float input; - float output; - } inv_biquad_filter_t; - - static inline float inv_q30_to_float(long q30) - { - return (float) q30 / ((float)(1L << 30)); - } - - static inline double inv_q30_to_double(long q30) - { - return (double) q30 / ((double)(1L << 30)); - } - - static inline float inv_q16_to_float(long q16) - { - return (float) q16 / (1L << 16); - } - - static inline double inv_q16_to_double(long q16) - { - return (double) q16 / (1L << 16); - } - - - - - long inv_q29_mult(long a, long b); - long inv_q30_mult(long a, long b); - - /* UMPL_ELIMINATE_64BIT Notes: - * An alternate implementation using float instead of long long accudoublemulators - * is provided for q29_mult and q30_mult. - * When long long accumulators are used and an alternate implementation is not - * available, we eliminate the entire function and header with a macro. - */ -#ifndef UMPL_ELIMINATE_64BIT - long inv_q30_div(long a, long b); - long inv_q_shift_mult(long a, long b, int shift); -#endif - - void inv_q_mult(const long *q1, const long *q2, long *qProd); - void inv_q_add(long *q1, long *q2, long *qSum); - void inv_q_normalize(long *q); - void inv_q_invert(const long *q, long *qInverted); - void inv_q_multf(const float *q1, const float *q2, float *qProd); - void inv_q_addf(const float *q1, const float *q2, float *qSum); - void inv_q_normalizef(float *q); - void inv_q_norm4(float *q); - void inv_q_invertf(const float *q, float *qInverted); - void inv_quaternion_to_rotation(const long *quat, long *rot); - unsigned char *inv_int32_to_big8(long x, unsigned char *big8); - long inv_big8_to_int32(const unsigned char *big8); - short inv_big8_to_int16(const unsigned char *big8); - short inv_little8_to_int16(const unsigned char *little8); - unsigned char *inv_int16_to_big8(short x, unsigned char *big8); - float inv_matrix_det(float *p, int *n); - void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); - double inv_matrix_detd(double *p, int *n); - void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); - float inv_wrap_angle(float ang); - float inv_angle_diff(float ang1, float ang2); - void inv_quaternion_to_rotation_vector(const long *quat, long *rot); - unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); - void inv_convert_to_body(unsigned short orientation, const long *input, long *output); - void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); - void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); - void inv_q_rotate(const long *q, const long *in, long *out); - void inv_vector_normalize(long *vec, int length); - uint32_t inv_checksum(const unsigned char *str, int len); - float inv_compass_angle(const long *compass, const long *grav, - const float *quat); - unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); - - static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) - { - return (long)((t1 - t2) / 1000000L); - } - - double quaternion_to_rotation_angle(const long *quat); - double inv_vector_norm(const float *x); - - void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); - float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); - void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); - void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); - -#ifdef __cplusplus -} -#endif -#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.c b/60xx/libsensors_iio/software/core/mllite/mpl.c deleted file mode 100644 index 9141cc6..0000000 --- a/60xx/libsensors_iio/software/core/mllite/mpl.c +++ /dev/null @@ -1,72 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-/**
- * @defgroup MPL mpl
- * @brief Motion Library - Start Point
- * Initializes MPL.
- *
- * @{
- * @file mpl.c
- * @brief MPL start point.
- */
-
-#include "storage_manager.h"
-#include "log.h"
-#include "mpl.h"
-#include "start_manager.h"
-#include "data_builder.h"
-#include "results_holder.h"
-#include "mlinclude.h"
-
-/**
- * @brief Initializes the MPL. Should be called first and once
- * @return Returns INV_SUCCESS if successful or an error code if not.
- */
-inv_error_t inv_init_mpl(void)
-{
- inv_init_storage_manager();
-
- /* initialize the start callback manager */
- INV_ERROR_CHECK(inv_init_start_manager());
-
- /* initialize the data builder */
- INV_ERROR_CHECK(inv_init_data_builder());
-
- INV_ERROR_CHECK(inv_enable_results_holder());
-
- return INV_SUCCESS;
-}
-
-const char ml_ver[] = "InvenSense MPL 5.1.2 beta RC9";
-
-/**
- * @brief used to get the MPL version.
- * @param version a string where the MPL version gets stored.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_get_version(char **version)
-{
- INVENSENSE_FUNC_START;
- /* cast out the const */
- *version = (char *)&ml_ver;
- return INV_SUCCESS;
-}
-
-/**
- * @brief Starts the MPL. Typically called after inv_init_mpl() or after a
- * inv_stop_mpl() to start the MPL back up an running.
- * @return INV_SUCCESS if successful or a non-zero error code otherwise.
- */
-inv_error_t inv_start_mpl(void)
-{
- INV_ERROR_CHECK(inv_execute_mpl_start_notification());
- return INV_SUCCESS;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.h b/60xx/libsensors_iio/software/core/mllite/mpl.h deleted file mode 100644 index a6b5ac7..0000000 --- a/60xx/libsensors_iio/software/core/mllite/mpl.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_MPL_H__ -#define INV_MPL_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_init_mpl(void); -inv_error_t inv_start_mpl(void); -inv_error_t inv_get_version(char **version); - -#ifdef __cplusplus -} -#endif - -#endif // INV_MPL_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.c b/60xx/libsensors_iio/software/core/mllite/results_holder.c deleted file mode 100644 index df58f40..0000000 --- a/60xx/libsensors_iio/software/core/mllite/results_holder.c +++ /dev/null @@ -1,522 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @defgroup Results_Holder results_holder - * @brief Motion Library - Results Holder - * Holds the data for MPL - * - * @{ - * @file results_holder.c - * @brief Results Holder for HAL. - */ - -#include <string.h> - -#include "results_holder.h" -#include "ml_math_func.h" -#include "mlmath.h" -#include "start_manager.h" -#include "data_builder.h" -#include "message_layer.h" -#include "log.h" - -// These 2 status bits are used to control when the 9 axis quaternion is updated -#define INV_COMPASS_CORRECTION_SET 1 -#define INV_6_AXIS_QUAT_SET 2 - -struct results_t { - long nav_quat[4]; - long gam_quat[4]; - inv_time_t nav_timestamp; - inv_time_t gam_timestamp; - long local_field[3]; /**< local earth's magnetic field */ - long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ - long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ - int acc_state; /**< Describes accel state */ - int got_accel_bias; /**< Flag describing if accel bias is known */ - long compass_bias_error[3]; /**< Error Squared */ - unsigned char motion_state; - unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ - long compass_count; /**< compass state internal counter */ - int got_compass_bias; /**< Flag describing if compass bias is known */ - int large_mag_field; /**< Flag describing if there is a large magnetic field */ - int compass_state; /**< Internal compass state */ - long status; - struct inv_sensor_cal_t *sensor; - float quat_confidence_interval; -}; -static struct results_t rh; - -/** @internal -* Store a quaternion more suitable for gaming. This quaternion is often determined -* using only gyro and accel. -* @param[in] quat Length 4, Quaternion scaled by 2^30 -*/ -void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) -{ - rh.status |= INV_6_AXIS_QUAT_SET; - memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); - rh.gam_timestamp = timestamp; -} - -/** @internal -* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. -* @param[in] data Quaternion Adjustment -* @param[in] timestamp Timestamp of when this is valid -*/ -void inv_set_compass_correction(const long *data, inv_time_t timestamp) -{ - rh.status |= INV_COMPASS_CORRECTION_SET; - memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); - rh.nav_timestamp = timestamp; -} - -/** @internal -* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. -* @param[out] data Quaternion Adjustment -* @param[out] timestamp Timestamp of when this is valid -*/ -void inv_get_compass_correction(long *data, inv_time_t *timestamp) -{ - memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); - *timestamp = rh.nav_timestamp; -} - -/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. - * @return Returns non-zero if there is a large magnetic field. - */ -int inv_get_large_mag_field() -{ - return rh.large_mag_field; -} - -/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. - * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. - */ -void inv_set_large_mag_field(int state) -{ - rh.large_mag_field = state; -} - -/** Gets the accel state set by inv_set_acc_state() - * @return accel state. - */ -int inv_get_acc_state() -{ - return rh.acc_state; -} - -/** Sets the accel state. See inv_get_acc_state() to get the value. - * @param[in] state value to set accel state to. - */ -void inv_set_acc_state(int state) -{ - rh.acc_state = state; - return; -} - -/** Returns the motion state -* @param[out] cntr Number of previous times a no motion event has occured in a row. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -int inv_get_motion_state(unsigned int *cntr) -{ - *cntr = rh.motion_state_counter; - return rh.motion_state; -} - -/** Sets the motion state - * @param[in] state motion state where INV_NO_MOTION is not moving - * and INV_MOTION is moving. - */ -void inv_set_motion_state(unsigned char state) -{ - long set; - if (state == rh.motion_state) { - if (state == INV_NO_MOTION) { - rh.motion_state_counter++; - } else { - rh.motion_state_counter = 0; - } - return; - } - rh.motion_state_counter = 0; - rh.motion_state = state; - /* Equivalent to set = state, but #define's may change. */ - if (state == INV_MOTION) - set = INV_MSG_MOTION_EVENT; - else - set = INV_MSG_NO_MOTION_EVENT; - inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); -} - -/** Sets the local earth's magnetic field -* @param[in] data Local earth's magnetic field in uT scaled by 2^16. -* Length = 3. Y typically points north, Z typically points down in -* northern hemisphere and up in southern hemisphere. -*/ -void inv_set_local_field(const long *data) -{ - memcpy(rh.local_field, data, sizeof(rh.local_field)); -} - -/** Gets the local earth's magnetic field -* @param[out] data Local earth's magnetic field in uT scaled by 2^16. -* Length = 3. Y typically points north, Z typically points down in -* northern hemisphere and up in southern hemisphere. -*/ -void inv_get_local_field(long *data) -{ - memcpy(data, rh.local_field, sizeof(rh.local_field)); -} - -/** Sets the compass sensitivity - * @param[in] data Length 3, sensitivity for each compass axis - * scaled such that 1.0 = 2^30. - */ -void inv_set_mag_scale(const long *data) -{ - memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); -} - -/** Gets the compass sensitivity - * @param[out] data Length 3, sensitivity for each compass axis - * scaled such that 1.0 = 2^30. - */ -void inv_get_mag_scale(long *data) -{ - memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); -} - -/** Gets gravity vector - * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_gravity(long *data) -{ - data[0] = - inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); - data[1] = - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); - data[2] = - (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) - - 1073741824L; - - return INV_SUCCESS; -} - -/** Returns a quaternion based only on gyro and accel. - * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_6axis_quaternion(long *data) -{ - memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); - return INV_SUCCESS; -} - -/** Returns a quaternion. - * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_quaternion(long *data) -{ - if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { - inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); - rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); - } - memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); - return INV_SUCCESS; -} - -/** Returns a quaternion. - * @param[out] data 9-axis quaternion. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_get_quaternion_float(float *data) -{ - long ldata[4]; - inv_error_t result = inv_get_quaternion(ldata); - data[0] = inv_q30_to_float(ldata[0]); - data[1] = inv_q30_to_float(ldata[1]); - data[2] = inv_q30_to_float(ldata[2]); - data[3] = inv_q30_to_float(ldata[3]); - return result; -} - -/** Returns a quaternion with accuracy and timestamp. - * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. - * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. - * @param[out] timestamp Timestamp of this quaternion in nanoseconds - */ -void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) -{ - inv_get_quaternion(data); - *timestamp = inv_get_last_timestamp(); - if (inv_get_compass_on()) { - *accuracy = inv_get_mag_accuracy(); - } else if (inv_get_gyro_on()) { - *accuracy = inv_get_gyro_accuracy(); - }else if (inv_get_accel_on()) { - *accuracy = inv_get_accel_accuracy(); - } else { - *accuracy = 0; - } -} - -/** Callback that gets called everytime there is new data. It is - * registered by inv_start_results_holder(). - * @param[in] sensor_cal New sensor data to process. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) -{ - rh.sensor = sensor_cal; - return INV_SUCCESS; -} - -/** Function to turn on this module. This is automatically called by - * inv_enable_results_holder(). Typically not called by users. - * @return Returns INV_SUCCESS if successful or an error code if not. - */ -inv_error_t inv_start_results_holder(void) -{ - inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, - INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); - return INV_SUCCESS; -} - -/** Initializes results holder. This is called automatically by the -* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but -* is typically not needed to be called by outside callers. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_init_results_holder(void) -{ - memset(&rh, 0, sizeof(rh)); - rh.mag_scale[0] = 1L<<30; - rh.mag_scale[1] = 1L<<30; - rh.mag_scale[2] = 1L<<30; - rh.compass_correction[0] = 1L<<30; - rh.gam_quat[0] = 1L<<30; - rh.nav_quat[0] = 1L<<30; - rh.quat_confidence_interval = (float)M_PI; - return INV_SUCCESS; -} - -/** Turns on storage of results. -*/ -inv_error_t inv_enable_results_holder() -{ - inv_error_t result; - result = inv_init_results_holder(); - if ( result ) { - return result; - } - - result = inv_register_mpl_start_notification(inv_start_results_holder); - return result; -} - -/** Sets state of if we know the accel bias. - * @return return 1 if we know the accel bias, 0 if not. - * it is set with inv_set_accel_bias_found() - */ -int inv_got_accel_bias() -{ - return rh.got_accel_bias; -} - -/** Sets whether we know the accel bias - * @param[in] state Set to 1 if we know the accel bias. - * Can be retrieved with inv_got_accel_bias() - */ -void inv_set_accel_bias_found(int state) -{ - rh.got_accel_bias = state; -} - -/** Sets state of if we know the compass bias. - * @return return 1 if we know the compass bias, 0 if not. - * it is set with inv_set_compass_bias_found() - */ -int inv_got_compass_bias() -{ - return rh.got_compass_bias; -} - -/** Sets whether we know the compass bias - * @param[in] state Set to 1 if we know the compass bias. - * Can be retrieved with inv_got_compass_bias() - */ -void inv_set_compass_bias_found(int state) -{ - rh.got_compass_bias = state; -} - -/** Sets the compass state. - * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). - */ -void inv_set_compass_state(int state) -{ - rh.compass_state = state; -} - -/** Get's the compass state - * @return the compass state that was set with inv_set_compass_state() - */ -int inv_get_compass_state() -{ - return rh.compass_state; -} - -/** Set compass bias error. See inv_get_compass_bias_error() - * @param[in] bias_error Set's how accurate we know the compass bias. It is the - * error squared. - */ -void inv_set_compass_bias_error(const long *bias_error) -{ - memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); -} - -/** Get's compass bias error. See inv_set_compass_bias_error() for setting. - * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. - */ -void inv_get_compass_bias_error(long *bias_error) -{ - memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * with gravity removed - * @param[out] data 3-element vector of accelerometer data in body frame - * with gravity removed - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_linear_accel(long *data) -{ - long gravity[3]; - - if (data != NULL) - { - inv_get_accel_set(data, NULL, NULL); - inv_get_gravity(gravity); - data[0] -= gravity[0] >> 14; - data[1] -= gravity[1] >> 14; - data[2] -= gravity[2] >> 14; - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * @param[out] data 3-element vector of accelerometer data in body frame - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_accel(long *data) -{ - if (data != NULL) { - inv_get_accel_set(data, NULL, NULL); - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of accelerometer float data - * @param[out] data 3-element vector of accelerometer float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_accel_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL && !inv_get_accel(tdata)) { - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @brief Returns 3-element vector of gyro float data - * @param[out] data 3-element vector of gyro float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_gyro_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL) { - inv_get_gyro_set(tdata, NULL, NULL); - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** Set 9 axis 95% heading confidence interval for quaternion -* @param[in] ci Confidence interval in radians. -*/ -void inv_set_heading_confidence_interval(float ci) -{ - rh.quat_confidence_interval = ci; -} - -/** Get 9 axis 95% heading confidence interval for quaternion -* @return Confidence interval in radians. -*/ -float inv_get_heading_confidence_interval(void) -{ - return rh.quat_confidence_interval; -} - -/** - * @brief Returns 3-element vector of linear accel float data - * @param[out] data 3-element vector of linear aceel float data - * @return INV_SUCCESS if successful - * INV_ERROR_INVALID_PARAMETER if invalid input pointer - */ -inv_error_t inv_get_linear_accel_float(float *data) -{ - long tdata[3]; - unsigned char i; - - if (data != NULL && !inv_get_linear_accel(tdata)) { - for (i = 0; i < 3; ++i) { - data[i] = ((float)tdata[i] / (1L << 16)); - } - return INV_SUCCESS; - } - else { - return INV_ERROR_INVALID_PARAMETER; - } -} - -/** - * @} - */ diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.h b/60xx/libsensors_iio/software/core/mllite/results_holder.h deleted file mode 100644 index 09da24e..0000000 --- a/60xx/libsensors_iio/software/core/mllite/results_holder.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_RESULTS_HOLDER_H__ -#define INV_RESULTS_HOLDER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#define INV_MOTION 0x0001 -#define INV_NO_MOTION 0x0002 - - /**************************************************************************/ - /* The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = */ - /* 2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even. */ - /* The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = */ - /* 2^ACC_MAG_SQR_SHIFT */ - /**************************************************************************/ -#define ACC_MAG_SQR_SHIFT 16 - -void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp); - -// States -#define SF_NORMAL 0 -#define SF_UNCALIBRATED 1 -#define SF_STARTUP_SETTLE 2 -#define SF_FAST_SETTLE 3 -#define SF_DISTURBANCE 4 -#define SF_SLOW_SETTLE 5 - -int inv_get_acc_state(); -void inv_set_acc_state(int state); -int inv_get_motion_state(unsigned int *cntr); -void inv_set_motion_state(unsigned char state); -inv_error_t inv_get_gravity(long *data); -inv_error_t inv_get_6axis_quaternion(long *data); -inv_error_t inv_get_quaternion(long *data); -inv_error_t inv_get_quaternion_float(float *data); -void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp); - -inv_error_t inv_enable_results_holder(); -inv_error_t inv_init_results_holder(void); - -/* Magnetic Field Parameters*/ -void inv_set_local_field(const long *data); -void inv_get_local_field(long *data); -void inv_set_mag_scale(const long *data); -void inv_get_mag_scale(long *data); -void inv_set_compass_correction(const long *data, inv_time_t timestamp); -void inv_get_compass_correction(long *data, inv_time_t *timestamp); -int inv_got_compass_bias(); -void inv_set_compass_bias_found(int state); -int inv_get_large_mag_field(); -void inv_set_large_mag_field(int state); -void inv_set_compass_state(int state); -int inv_get_compass_state(); -void inv_set_compass_bias_error(const long *bias_error); -void inv_get_compass_bias_error(long *bias_error); -inv_error_t inv_get_linear_accel(long *data); -inv_error_t inv_get_accel(long *data); -inv_error_t inv_get_accel_float(float *data); -inv_error_t inv_get_gyro_float(float *data); -inv_error_t inv_get_linear_accel_float(float *data); -void inv_set_heading_confidence_interval(float ci); -float inv_get_heading_confidence_interval(void); - -int inv_got_accel_bias(); -void inv_set_accel_bias_found(int state); - - -#ifdef __cplusplus -} -#endif - -#endif // INV_RESULTS_HOLDER_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.c b/60xx/libsensors_iio/software/core/mllite/start_manager.c deleted file mode 100644 index fb758e7..0000000 --- a/60xx/libsensors_iio/software/core/mllite/start_manager.c +++ /dev/null @@ -1,105 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-/**
- * @defgroup Start_Manager start_manager
- * @brief Motion Library - Start Manager
- * Start Manager
- *
- * @{
- * @file start_manager.c
- * @brief This handles all the callbacks when inv_start_mpl() is called.
- */
-
-
-#include <string.h>
-#include "log.h"
-#include "start_manager.h"
-
-typedef inv_error_t (*inv_start_cb_func)();
-struct inv_start_cb_t {
- int num_cb;
- inv_start_cb_func start_cb[INV_MAX_START_CB];
-};
-
-static struct inv_start_cb_t inv_start_cb;
-
-/** Initilize the start manager. Typically called by inv_start_mpl();
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_init_start_manager(void)
-{
- memset(&inv_start_cb, 0, sizeof(inv_start_cb));
- return INV_SUCCESS;
-}
-
-/** Removes a callback from start notification
-* @param[in] start_cb function to remove from start notification
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- int kk;
-
- for (kk=0; kk<inv_start_cb.num_cb; ++kk) {
- if (inv_start_cb.start_cb[kk] == start_cb) {
- // Found the match
- if (kk != (inv_start_cb.num_cb-1)) {
- memmove(&inv_start_cb.start_cb[kk],
- &inv_start_cb.start_cb[kk+1],
- (inv_start_cb.num_cb-kk-1)*sizeof(inv_start_cb_func));
- }
- inv_start_cb.num_cb--;
- return INV_SUCCESS;
- }
- }
- return INV_ERROR_INVALID_PARAMETER;
-}
-
-/** Register a callback to receive when inv_start_mpl() is called.
-* @param[in] start_cb Function callback that will be called when inv_start_mpl() is
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void))
-{
- if (inv_start_cb.num_cb >= INV_MAX_START_CB)
- return INV_ERROR_INVALID_PARAMETER;
-
- inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb;
- inv_start_cb.num_cb++;
- return INV_SUCCESS;
-}
-
-/** Callback all the functions that want to be notified when inv_start_mpl() was
-* called.
-* @return Returns INV_SUCCESS if successful or an error code if not.
-*/
-inv_error_t inv_execute_mpl_start_notification(void)
-{
- inv_error_t result,first_error;
- int kk;
-
- first_error = INV_SUCCESS;
-
- for (kk = 0; kk < inv_start_cb.num_cb; ++kk) {
- result = inv_start_cb.start_cb[kk]();
- if (result && (first_error == INV_SUCCESS)) {
- first_error = result;
- }
- }
- return first_error;
-}
-
-/**
- * @}
- */
diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.h b/60xx/libsensors_iio/software/core/mllite/start_manager.h deleted file mode 100644 index 899e3f5..0000000 --- a/60xx/libsensors_iio/software/core/mllite/start_manager.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#ifndef INV_START_MANAGER_H__ -#define INV_START_MANAGER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" - -/** Max number of start callbacks we can handle. */ -#define INV_MAX_START_CB 20 - -inv_error_t inv_init_start_manager(void); -inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void)); -inv_error_t inv_execute_mpl_start_notification(void); -inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void)); - -#ifdef __cplusplus -} -#endif -#endif // INV_START_MANAGER_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c deleted file mode 100644 index 3e4e619..0000000 --- a/60xx/libsensors_iio/software/core/mllite/storage_manager.c +++ /dev/null @@ -1,204 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/** - * @defgroup Storage_Manager storage_manager - * @brief Motion Library - Stores Data for functions. - * - * - * @{ - * @file storage_manager.c - * @brief Load and Store Manager. - */ - -#include <string.h> - -#include "storage_manager.h" -#include "log.h" -#include "ml_math_func.h" -#include "mlmath.h" - -/* Must be changed if the format of storage changes */ -#define DEFAULT_KEY 29681 - -typedef inv_error_t (*load_func_t)(const unsigned char *data); -typedef inv_error_t (*save_func_t)(unsigned char *data); -/** Max number of entites that can be stored */ -#define NUM_STORAGE_BOXES 20 - -struct data_header_t { - long size; - uint32_t checksum; - unsigned int key; -}; - -struct data_storage_t { - int num; /**< Number of differnt save entities */ - size_t total_size; /**< Size in bytes to store non volatile data */ - load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ - save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ - struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ -}; -static struct data_storage_t ds; - -/** Should be called once before using any of the storage methods. Typically -* called first by inv_init_mpl().*/ -void inv_init_storage_manager() -{ - memset(&ds, 0, sizeof(ds)); - ds.total_size = sizeof(struct data_header_t); -} - -/** Used to register your mechanism to load and store non-volative data. This should typical be -* called during the enable function for your feature. -* @param[in] load_func function pointer you will use to receive data that was stored for you. -* @param[in] save_func function pointer you will use to save any data you want saved to -* non-volatile memory between runs. -* @param[in] size The size in bytes of the amount of data you want loaded and saved. -* @param[in] key The key associated with your data type should be unique across MPL. -* The key should change when your type of data for storage changes. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), - inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) -{ - int kk; - // Check if this has been registered already - for (kk=0; kk<ds.num; ++kk) { - if (key == ds.hd[kk].key) { - return INV_ERROR_INVALID_PARAMETER; - } - } - // Make sure there is room - if (ds.num >= NUM_STORAGE_BOXES) { - return INV_ERROR_INVALID_PARAMETER; - } - // Add to list - ds.hd[ds.num].key = key; - ds.hd[ds.num].size = size; - ds.load[ds.num] = load_func; - ds.save[ds.num] = save_func; - ds.total_size += size + sizeof(struct data_header_t); - ds.num++; - - return INV_SUCCESS; -} - -/** Returns the memory size needed to perform a store -* @param[out] size Size in bytes of memory needed to store. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_get_mpl_state_size(size_t *size) -{ - *size = ds.total_size; - return INV_SUCCESS; -} - -/** @internal - * Finds key in ds.hd[] array and returns location - * @return location where key exists in array, -1 if not found. - */ -static int inv_find_entry(unsigned int key) -{ - int kk; - for (kk=0; kk<ds.num; ++kk) { - if (key == ds.hd[kk].key) { - return kk; - } - } - return -1; -} - -/** This function takes a block of data that has been saved in non-volatile memory and pushes -* to the proper locations. Multiple error checks are performed on the data. -* @param[in] data Data that was saved to be loaded up by MPL -* @param[in] length Length of data vector in bytes -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) -{ - struct data_header_t *hd; - int entry; - uint32_t checksum; - long len; - - len = length; // Important so we get negative numbers - if (len < sizeof(struct data_header_t)) - return INV_ERROR_CALIBRATION_LOAD; // No data - hd = (struct data_header_t *)data; - if (hd->key != DEFAULT_KEY) - return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption - len = MIN(hd->size, len); - len = hd->size; - len -= sizeof(struct data_header_t); - data += sizeof(struct data_header_t); - checksum = inv_checksum(data, len); - if (checksum != hd->checksum) - return INV_ERROR_CALIBRATION_LOAD; // Data corruption - - while (len > (long)sizeof(struct data_header_t)) { - hd = (struct data_header_t *)data; - entry = inv_find_entry(hd->key); - data += sizeof(struct data_header_t); - len -= sizeof(struct data_header_t); - if (entry >= 0 && len >= hd->size) { - if (hd->size == ds.hd[entry].size) { - checksum = inv_checksum(data, hd->size); - if (checksum == hd->checksum) { - ds.load[entry](data); - } else { - return INV_ERROR_CALIBRATION_LOAD; - } - } - } - len -= hd->size; - if (len >= 0) - data = data + hd->size; - } - - return INV_SUCCESS; -} - -/** This function fills up a block of memory to be stored in non-volatile memory. -* @param[out] data Place to store data, size of sz, must be at least size -* returned by inv_get_mpl_state_size() -* @param[in] sz Size of data. -* @return Returns INV_SUCCESS if successful or an error code if not. -*/ -inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) -{ - unsigned char *cur; - int kk; - struct data_header_t *hd; - - if (sz >= ds.total_size) { - cur = data + sizeof(struct data_header_t); - for (kk = 0; kk < ds.num; ++kk) { - hd = (struct data_header_t *)cur; - cur += sizeof(struct data_header_t); - ds.save[kk](cur); - hd->checksum = inv_checksum(cur, ds.hd[kk].size); - hd->size = ds.hd[kk].size; - hd->key = ds.hd[kk].key; - cur += ds.hd[kk].size; - } - } else { - return INV_ERROR_CALIBRATION_LOAD; - } - - hd = (struct data_header_t *)data; - hd->checksum = inv_checksum(data + sizeof(struct data_header_t), - ds.total_size - sizeof(struct data_header_t)); - hd->key = DEFAULT_KEY; - hd->size = ds.total_size; - - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.h b/60xx/libsensors_iio/software/core/mllite/storage_manager.h deleted file mode 100644 index 7255591..0000000 --- a/60xx/libsensors_iio/software/core/mllite/storage_manager.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#include "mltypes.h" - -#ifndef INV_STORAGE_MANAGER_H__ -#define INV_STORAGE_MANAGER_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_register_load_store( - inv_error_t (*load_func)(const unsigned char *data), - inv_error_t (*save_func)(unsigned char *data), - size_t size, unsigned int key); -void inv_init_storage_manager(void); - -inv_error_t inv_get_mpl_state_size(size_t *size); -inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len); -inv_error_t inv_save_mpl_states(unsigned char *data, size_t len); - -#ifdef __cplusplus -} -#endif - -#endif /* INV_STORAGE_MANAGER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h deleted file mode 100644 index 5a53213..0000000 --- a/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h +++ /dev/null @@ -1,38 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id$
- *
- ******************************************************************************/
-
-#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__
-#define MLDMP_ACCEL_AUTO_CALIBRATION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_in_use_auto_calibration(void);
-inv_error_t inv_disable_in_use_auto_calibration(void);
-inv_error_t inv_stop_in_use_auto_calibration(void);
-inv_error_t inv_start_in_use_auto_calibration(void);
-inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled);
-inv_error_t inv_init_in_use_auto_calibration(void);
-void inv_init_accel_maxmin(void);
-void inv_record_good_accel_maxmin(void);
-int inv_get_accel_bias_stage();
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__
-
diff --git a/60xx/libsensors_iio/software/core/mpl/authenticate.h b/60xx/libsensors_iio/software/core/mpl/authenticate.h deleted file mode 100644 index d7c803b..0000000 --- a/60xx/libsensors_iio/software/core/mpl/authenticate.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/******************************************************************************* - * - * $Id$ - * - ******************************************************************************/ - -#ifndef MLDMP_AUTHENTICATE_H__ -#define MLDMP_AUTHENTICATE_H__ - -#include "invensense.h" - -inv_error_t inv_check_key(void); - -#endif /* MLDMP_AUTHENTICATE_H__ */ diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so Binary files differdeleted file mode 100644 index fbd346f..0000000 --- a/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so +++ /dev/null diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk deleted file mode 100644 index 2e15205..0000000 --- a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk +++ /dev/null @@ -1,88 +0,0 @@ -MPL_LIB_NAME ?= mplmpu -LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- -COMP ?= $(CROSS)gcc -LINK ?= $(CROSS)gcc - -OBJFOLDER = $(CURDIR)/obj - -INV_ROOT = ../../../../.. -MLLITE_DIR = $(INV_ROOT)/software/core/mllite -MPL_DIR = $(INV_ROOT)/software/core/mpl - -include $(INV_ROOT)/software/build/android/common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += $(ANDROID_COMPILE) -CFLAGS += -Wall -CFLAGS += -fpic -CFLAGS += -nostdlib -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -CFLAGS += -DLINUX -CFLAGS += -DANDROID -CFLAGS += -mthumb-interwork -CFLAGS += -fno-exceptions -CFLAGS += -ffunction-sections -CFLAGS += -funwind-tables -CFLAGS += -fstack-protector -CFLAGS += -fno-short-enums -CFLAGS += -fmessage-length=0 -CFLAGS += $(INV_INCLUDES) -CFLAGS += $(INV_DEFINES) - -LLINK = -lc -LLINK += -lm -LLINK += -lutils -LLINK += -lcutils -LLINK += -lgcc -LLINK += -ldl - -LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += -shared -LFLAGS += -Wl,-soname,$(LIBRARY) -LFLAGS += -Wl,-shared,-Bsymbolic -LFLAGS += $(ANDROID_LINK) -LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib - -#################################################################################################### -## sources - -INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) - -#INV_SOURCES, VPATH provided by Makefile.filelist -include ../filelist.mk - -INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) -INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all mpl clean cleanall - -all: mpl - -mpl: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) - @$(call echo_in_colors, "\n<linking $(LIBRARY) with objects $(INV_OBJS_DST)\n") - $(LINK) $(LFLAGS) -o $(LIBRARY) $(INV_OBJS_DST) $(LLINK) $(INV_LIBS) $(LLINK) - -$(OBJFOLDER) : - @$(call echo_in_colors, "\n<creating object's folder 'obj/'>\n") - mkdir obj - -$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n<compile $< to $(OBJFOLDER)/$(notdir $@)>\n") - $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/60xx/libsensors_iio/software/core/mpl/build/filelist.mk b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk deleted file mode 100644 index e18003a..0000000 --- a/60xx/libsensors_iio/software/core/mpl/build/filelist.mk +++ /dev/null @@ -1,46 +0,0 @@ -#### filelist.mk for mpl #### - -# headers only -HEADERS := $(MPL_DIR)/mpu.h - -# headers for sources -HEADERS := $(MPL_DIR)/fast_no_motion.h -HEADERS += $(MPL_DIR)/fusion_9axis.h -HEADERS += $(MPL_DIR)/motion_no_motion.h -HEADERS += $(MPL_DIR)/no_gyro_fusion.h -HEADERS += $(MPL_DIR)/quaternion_supervisor.h -HEADERS += $(MPL_DIR)/gyro_tc.h -HEADERS += $(MPL_DIR)/authenticate.h -HEADERS += $(MPL_DIR)/accel_auto_cal.h -HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h -HEADERS += $(MPL_DIR)/compass_vec_cal.h -HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h -HEADERS += $(MPL_DIR)/mag_disturb.h -HEADERS += $(MPL_DIR)/mag_disturb_protected.h -HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h -HEADERS += $(MPL_DIR)/heading_from_gyro.h -HEADERS += $(MPL_DIR)/compass_fit.h -HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h -#HEADERS += $(MPL_DIR)/auto_calibration.h - -# sources -SOURCES := $(MPL_DIR)/fast_no_motion.c -SOURCES += $(MPL_DIR)/fusion_9axis.c -SOURCES += $(MPL_DIR)/motion_no_motion.c -SOURCES += $(MPL_DIR)/no_gyro_fusion.c -SOURCES += $(MPL_DIR)/quaternion_supervisor.c -SOURCES += $(MPL_DIR)/gyro_tc.c -SOURCES += $(MPL_DIR)/authenticate.c -SOURCES += $(MPL_DIR)/accel_auto_cal.c -SOURCES += $(MPL_DIR)/compass_vec_cal.c -SOURCES += $(MPL_DIR)/mag_disturb.c -SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c -SOURCES += $(MPL_DIR)/heading_from_gyro.c -SOURCES += $(MPL_DIR)/compass_fit.c -SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c -#SOURCES += $(MPL_DIR)/auto_calibration.c - -INV_SOURCES += $(SOURCES) - -VPATH = $(MPL_DIR) - diff --git a/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h deleted file mode 100644 index 4f01fc2..0000000 --- a/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h +++ /dev/null @@ -1,31 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_COMPASSBIASWGYRO_H__ -#define MLDMP_COMPASSBIASWGYRO_H__ -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_compass_bias_w_gyro(); - inv_error_t inv_disable_compass_bias_w_gyro(); - void inv_init_compass_bias_w_gyro(); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_COMPASSBIASWGYRO_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/compass_fit.h b/60xx/libsensors_iio/software/core/mpl/compass_fit.h deleted file mode 100644 index be3cce8..0000000 --- a/60xx/libsensors_iio/software/core/mpl/compass_fit.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -#ifndef INV_MLDMP_COMPASSFIT_H__ -#define INV_MLDMP_COMPASSFIT_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -void inv_init_compass_fit(void); -inv_error_t inv_enable_compass_fit(void); -inv_error_t inv_disable_compass_fit(void); -inv_error_t inv_start_compass_fit(void); -inv_error_t inv_stop_compass_fit(void); - -#ifdef __cplusplus -} -#endif - - -#endif // INV_MLDMP_COMPASSFIT_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h deleted file mode 100644 index a3e044c..0000000 --- a/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -#ifndef COMPASS_ONLY_CAL_H__ -#define COMPASS_ONLY_CAL_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_enable_vector_compass_cal(); -inv_error_t inv_disable_vector_compass_cal(); -inv_error_t inv_start_vector_compass_cal(void); -inv_error_t inv_stop_vector_compass_cal(void); -void inv_vector_compass_cal_sensitivity(float sens); -inv_error_t inv_init_vector_compass_cal(); - -#ifdef __cplusplus -} -#endif - -#endif // COMPASS_ONLY_CAL_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h deleted file mode 100644 index c553926..0000000 --- a/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h +++ /dev/null @@ -1,46 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_FAST_NO_MOTION_H__ -#define MLDMP_FAST_NO_MOTION_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_fast_nomot(void); - inv_error_t inv_disable_fast_nomot(void); - inv_error_t inv_start_fast_nomot(void);
- inv_error_t inv_stop_fast_nomot(void);
- inv_error_t inv_init_fast_nomot(void);
- void inv_set_default_number_of_samples(int N);
- inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled);
- inv_error_t inv_update_fast_nomot(long *gyro);
-
- void inv_get_fast_nomot_accel_param(long *cntr, long long *param);
- void inv_get_fast_nomot_compass_param(long *cntr, long long *param);
- void inv_set_fast_nomot_accel_threshold(long long thresh);
- void inv_set_fast_nomot_compass_threshold(long long thresh);
- void int_set_fast_nomot_gyro_threshold(long long thresh);
-
- void inv_fnm_debug_print(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif // MLDMP_FAST_NO_MOTION_H__
-
diff --git a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h deleted file mode 100644 index 1ba1ebb..0000000 --- a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h +++ /dev/null @@ -1,37 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_FUSION9AXIS_H__ -#define MLDMP_FUSION9AXIS_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - void inv_init_9x_fusion(void); - inv_error_t inv_9x_fusion_state_change(unsigned char newState); - inv_error_t inv_enable_9x_sensor_fusion(void); - inv_error_t inv_disable_9x_sensor_fusion(void); - inv_error_t inv_start_9x_sensor_fusion(void); - inv_error_t inv_stop_9x_sensor_fusion(void); - inv_error_t inv_9x_fusion_set_mag_fb(double fb);
- inv_error_t inv_9x_fusion_enable_jitter_reduction(int en);
- -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_FUSION9AXIS_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h deleted file mode 100644 index 69918d5..0000000 --- a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h +++ /dev/null @@ -1,43 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef _GYRO_TC_H_ -#define _GYRO_TC_H_ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_enable_gyro_tc(void); -inv_error_t inv_disable_gyro_tc(void); -inv_error_t inv_start_gyro_tc(void);
-inv_error_t inv_stop_gyro_tc(void); - -inv_error_t inv_get_gyro_ts(long *data); -inv_error_t inv_set_gyro_ts(long *data); - -inv_error_t inv_init_gyro_ts(void); - -inv_error_t inv_set_gtc_max_temp(long data); -inv_error_t inv_set_gtc_min_temp(long data); - -inv_error_t inv_print_gtc_data(void); - -#ifdef __cplusplus -} -#endif - -#endif /* _GYRO_TC_H */ - diff --git a/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h deleted file mode 100644 index 09ecc42..0000000 --- a/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef _HEADING_FROM_GYRO_H_ -#define _HEADING_FROM_GYRO_H_ -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_heading_from_gyro(void); - inv_error_t inv_disable_heading_from_gyro(void); - void inv_init_heading_from_gyro(void); - inv_error_t inv_start_heading_from_gyro(void); - inv_error_t inv_stop_heading_from_gyro(void); - -#ifdef __cplusplus -} -#endif - - -#endif /* _HEADING_FROM_GYRO_H_ */ diff --git a/60xx/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h deleted file mode 100644 index 6620bbf..0000000 --- a/60xx/libsensors_iio/software/core/mpl/inv_math.h +++ /dev/null @@ -1,8 +0,0 @@ -/* math.h has many functions and defines that are not consistent across
-* platforms. This address that */
-
-#ifdef _WINDOWS
-#define _USE_MATH_DEFINES
-#endif
-
-#include <math.h>
diff --git a/60xx/libsensors_iio/software/core/mpl/invensense_adv.h b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h deleted file mode 100644 index 12932c9..0000000 --- a/60xx/libsensors_iio/software/core/mpl/invensense_adv.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - $License: - Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -/* - Main header file for Invensense's Advanced library. -*/ - -#include "accel_auto_cal.h" -#include "compass_bias_w_gyro.h" -#include "compass_fit.h" -#include "compass_vec_cal.h" -#include "fast_no_motion.h" -#include "fusion_9axis.h" -#include "gyro_tc.h" -#include "heading_from_gyro.h" -#include "mag_disturb.h" -#include "motion_no_motion.h" -#include "no_gyro_fusion.h" -#include "quaternion_supervisor.h" -#include "mag_disturb.h" -#include "quat_accuracy_monitor.h" -#include "shake.h" diff --git a/60xx/libsensors_iio/software/core/mpl/mag_disturb.h b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h deleted file mode 100644 index 38df919..0000000 --- a/60xx/libsensors_iio/software/core/mpl/mag_disturb.h +++ /dev/null @@ -1,37 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ - -#ifndef MLDMP_MAGDISTURB_H__ -#define MLDMP_MAGDISTURB_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat, - const long *compass, const long *gravity); - - void inv_track_dip_angle(int mode, float currdip); - - inv_error_t inv_enable_magnetic_disturbance(void); - inv_error_t inv_disable_magnetic_disturbance(void); - int inv_get_magnetic_disturbance_state(); - inv_error_t inv_set_magnetic_disturbance(int time_ms); - inv_error_t inv_disable_dip_tracking(void); - inv_error_t inv_enable_dip_tracking(void); - inv_error_t inv_init_magnetic_disturbance(void);
- - float Mag3ofNormalizedLong(const long *x);
-
-#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_MAGDISTURB_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h deleted file mode 100644 index 01cf1c0..0000000 --- a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h +++ /dev/null @@ -1,28 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ -#ifndef INV_MOTION_NO_MOTION_H__
-#define INV_MOTION_NO_MOTION_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_motion_no_motion(void);
-inv_error_t inv_disable_motion_no_motion(void);
-inv_error_t inv_init_motion_no_motion(void);
-inv_error_t inv_start_motion_no_motion(void);
-inv_error_t inv_stop_motion_no_motion(void);
-
-inv_error_t inv_set_no_motion_time(long time_ms);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_MOTION_NO_MOTION_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h deleted file mode 100644 index 38d5690..0000000 --- a/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */ - - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_NOGYROFUSION_H__ -#define MLDMP_NOGYROFUSION_H__ -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_no_gyro_fusion(void); - inv_error_t inv_disable_no_gyro_fusion(void); - inv_error_t inv_start_no_gyro_fusion(void);
- inv_error_t inv_start_no_gyro_fusion(void);
- inv_error_t inv_init_no_gyro_fusion(void); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_NOGYROFUSION_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h deleted file mode 100644 index 3c6a2c1..0000000 --- a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h +++ /dev/null @@ -1,71 +0,0 @@ -/*
- quat_accuracy_monitor.h
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-
-/*******************************************************************************
- *
- * $Id:$
- *
- ******************************************************************************/
-
-#ifndef QUAT_ACCURARCY_MONITOR_H__
-#define QUAT_ACCURARCY_MONITOR_H__
-
-#include "mltypes.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-enum accuracy_signal_type_e {
- TYPE_NAV_QUAT,
- TYPE_GAM_QUAT,
- TYPE_NAV_QUAT_ADVANCED,
- TYPE_GAM_QUAT_ADVANCED,
- TYPE_NAV_QUAT_BASIC,
- TYPE_GAM_QUAT_BASIC,
- TYPE_MAG,
- TYPE_GYRO,
- TYPE_ACCEL,
-};
-
-inv_error_t inv_init_quat_accuracy_monitor(void);
-
-void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold);
-double get_accuracy_threshold(enum accuracy_signal_type_e type);
-void set_accuracy_weight(enum accuracy_signal_type_e type, int weight);
-int get_accuracy_weight(enum accuracy_signal_type_e type);
-
-int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type);
-
-void inv_reset_quat_accuracy(void);
-double get_6axis_correction_term(void);
-double get_9axis_correction_term(void);
-int get_9axis_accuracy_state();
-
-void set_6axis_error_average(double value);
-double get_6axis_error_bound(void);
-double get_compass_correction(void);
-double get_9axis_error_bound(void);
-
-float get_confidence_interval(void);
-void set_compass_uncertainty(float value);
-
-inv_error_t inv_enable_quat_accuracy_monitor(void);
-inv_error_t inv_disable_quat_accuracy_monitor(void);
-inv_error_t inv_start_quat_accuracy_monitor(void);
-inv_error_t inv_stop_quat_accuracy_monitor(void);
-
-double get_compassNgravity(void);
-double get_init_compassNgravity(void);
-
-float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // QUAT_ACCURARCY_MONITOR_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h deleted file mode 100644 index 4fa36b0..0000000 --- a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h +++ /dev/null @@ -1,27 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_QUATERNION_SUPERVISOR_H__
-#define INV_QUATERNION_SUPERVISOR_H__
-
-#include "mltypes.h"
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-inv_error_t inv_enable_quaternion(void);
-inv_error_t inv_disable_quaternion(void);
-inv_error_t inv_init_quaternion(void);
-inv_error_t inv_start_quaternion(void);
-void inv_set_quaternion(long *quat); -
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_QUATERNION_SUPERVISOR_H__
diff --git a/60xx/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h deleted file mode 100644 index 67acb7b..0000000 --- a/60xx/libsensors_iio/software/core/mpl/shake.h +++ /dev/null @@ -1,94 +0,0 @@ -/*
- $License:
- Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
- See included License.txt for License information.
- $
- */
-#ifndef INV_SHAKE_H__
-#define INV_SHAKE_H__
-
-#include "mltypes.h"
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
- /* ------------ */
- /* - Defines. - */
- /* ------------ */
-
- #define STATE_ZERO 0
- #define STATE_INIT_1 1
- #define STATE_INIT_2 2
- #define STATE_DETECT 3
-
- struct t_shake_config_params {
- long shake_time_min_ms;
- long shake_time_max_ms;
- long shake_time_min;
- long shake_time_max;
- unsigned char shake_time_set;
- long shake_time_saved;
- float shake_deriv_thr;
- int zero_cross_thr;
- float accel_delta_min;
- float accel_delta_max;
- unsigned char interp_enable;
- };
-
- struct t_shake_state_params {
- unsigned char state;
- float accel_peak_high;
- float accel_peak_low;
- float accel_range;
- int num_zero_cross;
- short curr_shake_time;
- int deriv_major_change;
- int deriv_major_sign;
- float accel_buffer[200];
- float delta_buffer[200];
- };
-
- struct t_shake_data_params {
- float accel_prev;
- float accel_curr;
- float delta_prev;
- float delta_curr;
- float delta_prev_buffer;
- };
-
- struct t_shake_results {
- //unsigned char shake_int;
- int shake_number;
- };
-
- struct t_shake_cb {
- void (*shake_callback)(struct t_shake_results *shake_results);
- };
-
-
- /* --------------------- */
- /* - Function p-types. - */
- /* --------------------- */
- inv_error_t inv_enable_shake(void);
- inv_error_t inv_disable_shake(void);
- inv_error_t inv_init_shake(void);
- inv_error_t inv_start_shake(void);
- int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results));
- void inv_config_shake_time_params(long sample_time_ms);
- void inv_set_shake_accel_delta_min(float accel_g);
- void inv_set_shake_accel_delta_max(float accel_g);
- void inv_set_shake_zero_cross_thresh(int num_zero_cross);
- void inv_set_shake_deriv_thresh(float shake_deriv_thresh);
- void inv_set_shake_time_min_ms(long time_ms);
- void inv_set_shake_time_max_ms(long time_ms);
- void inv_enable_shake_data_interpolation(unsigned char en);
-
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // INV_SHAKE__
\ No newline at end of file diff --git a/60xx/mlsdk/Android.mk b/60xx/mlsdk/Android.mk deleted file mode 100644 index 929a776..0000000 --- a/60xx/mlsdk/Android.mk +++ /dev/null @@ -1,101 +0,0 @@ -LOCAL_PATH := $(call my-dir) - -ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false) - -include $(CLEAR_VARS) -LOCAL_MODULE_TAGS := optional - -LOCAL_MODULE := libmlplatform -#modify these to point to the mpl source installation -MLSDK_ROOT = . -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform/linux - -LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID -LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050 -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR) -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/kernel -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite - -ML_SOURCES := \ - $(MLPLATFORM_DIR)/mlos_linux.c \ - $(MLPLATFORM_DIR)/mlsl_linux_mpu.c - -LOCAL_SRC_FILES := $(ML_SOURCES) - -LOCAL_SHARED_LIBRARIES := liblog libm libutils libcutils -include $(BUILD_SHARED_LIBRARY) - -include $(CLEAR_VARS) -LOCAL_MODULE := libmllite -LOCAL_MODULE_TAGS := optional -#modify these to point to the mpl source installation -MLSDK_ROOT = . -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform -MLLITE_DIR = $(MLSDK_ROOT)/mllite -MPL_DIR = $(MLSDK_ROOT)/mldmp - -LOCAL_CFLAGS += -DNDEBUG -LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID -LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050 -LOCAL_CFLAGS += -DUNICODE -D_UNICODE -DSK_RELEASE -LOCAL_CFLAGS += -DI2CDEV=\"/dev/mpu\" -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MPL_DIR) -I$(LOCAL_PATH)/$(MLLITE_DIR) -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/include -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlutils -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlapps/common -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite/akmd -LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/linux - -# optionally apply the compass filter. this is set in -# BoardConfig.mk -ifeq ($(BOARD_INVENSENSE_APPLY_COMPASS_NOISE_FILTER),true) -LOCAL_CFLAGS += -DAPPLY_COMPASS_FILTER -endif - -ML_SOURCES = \ - $(MLLITE_DIR)/accel.c \ - $(MLLITE_DIR)/compass.c \ - $(MLLITE_DIR)/pressure.c \ - $(MLLITE_DIR)/mldl_cfg_mpu.c \ - $(MLLITE_DIR)/dmpDefault.c \ - $(MLLITE_DIR)/ml.c \ - $(MLLITE_DIR)/mlarray.c \ - $(MLLITE_DIR)/mlarray_legacy.c \ - $(MLLITE_DIR)/mlFIFO.c \ - $(MLLITE_DIR)/mlFIFOHW.c \ - $(MLLITE_DIR)/mlMathFunc.c \ - $(MLLITE_DIR)/ml_stored_data.c \ - $(MLLITE_DIR)/mlcontrol.c \ - $(MLLITE_DIR)/mldl.c \ - $(MLLITE_DIR)/mldmp.c \ - $(MLLITE_DIR)/mlstates.c \ - $(MLLITE_DIR)/mlsupervisor.c \ - $(MLLITE_DIR)/mlBiasNoMotion.c \ - $(MLLITE_DIR)/mlSetGyroBias.c \ - \ - $(MLLITE_DIR)/ml_mputest.c \ - $(MLSDK_ROOT)/mlutils/mputest.c \ - $(MLSDK_ROOT)/mlutils/checksum.c - - -ifeq ($(HARDWARE),M_HW) - ML_SOURCES += $(MLLITE_DIR)/accel/mantis.c -endif - -LOCAL_SRC_FILES := $(ML_SOURCES) -LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform -include $(BUILD_SHARED_LIBRARY) - -#This makes an .so from our .a -#include $(CLEAR_VARS) -#LOCAL_MODULE := libmpl -#LOCAL_MODULE_TAGS := optional -#LOCAL_SRC_FILES := mlsdk/mldmp/mpl/android/libmpl.a -#LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform libmllite -#LOCAL_WHOLE_STATIC_LIBRARIES := libmpl -#LOCAL_PREBUILT_LIBS := mlsdk/mldmp/mpl/android/libmpl.a -#include $(BUILD_SHARED_LIBRARY) -#include $(BUILD_MULTI_PREBUILT) - -endif diff --git a/60xx/mlsdk/mllite/accel.c b/60xx/mlsdk/mllite/accel.c deleted file mode 100644 index 60b8d6c..0000000 --- a/60xx/mlsdk/mllite/accel.c +++ /dev/null @@ -1,189 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: accel.c 4595 2011-01-25 01:43:03Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup ACCELDL - * @brief Motion Library - Accel Driver Layer. - * Provides the interface to setup and handle an accel - * connected to either the primary or the seconday I2C interface - * of the gyroscope. - * - * @{ - * @file accel.c - * @brief Accel setup and handling methods. -**/ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> - -#include "ml.h" -#include "mlinclude.h" -#include "dmpKey.h" -#include "mlFIFO.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mlMathFunc.h" -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-accel" - -#define ACCEL_DEBUG 0 - -/* --------------------- */ -/* - Global Variables. - */ -/* --------------------- */ - -/* --------------------- */ -/* - Static Variables. - */ -/* --------------------- */ - -/* --------------- */ -/* - Prototypes. - */ -/* --------------- */ - -/* -------------- */ -/* - Externs. - */ -/* -------------- */ - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Used to determine if an accel is configured and - * used by the MPL. - * @return INV_SUCCESS if the accel is present. - */ -unsigned char inv_accel_present(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->accel && - NULL != mldl_cfg->accel->resume && - mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) - return TRUE; - else - return FALSE; -} - -/** - * @brief Query the accel slave address. - * @return The 7-bit accel slave address. - */ -unsigned char inv_get_slave_addr(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pdata) - return mldl_cfg->pdata->accel.address; - else - return 0; -} - -/** - * @brief Get the ID of the accel in use. - * @return ID of the accel in use. - */ -unsigned short inv_get_accel_id(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->accel) { - return mldl_cfg->accel->id; - } - return ID_INVALID; -} - -/** - * @brief Get a sample of accel data from the device. - * @param data - * the buffer to store the accel raw data for - * X, Y, and Z axes. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_accel_data(long *data) -{ - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - inv_error_t result; - unsigned char raw_data[2 * ACCEL_NUM_AXES]; - long tmp[ACCEL_NUM_AXES]; - int ii; - signed char *mtx = mldl_cfg->pdata->accel.orientation; - char accelId = mldl_cfg->accel->id; - - if (NULL == data) - return INV_ERROR_INVALID_PARAMETER; - - if (mldl_cfg->accel->read_len > sizeof(raw_data)) - return INV_ERROR_ASSERTION_FAILURE; - - result = (inv_error_t) inv_mpu_read_accel(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), - raw_data); - if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { - return result; - } - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { - if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) { - tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256; - tmp[ii] += (long)((unsigned char)raw_data[2 * ii]); - } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) || - (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) { - tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256; - tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]); - if (accelId == ACCEL_ID_KXSD9) { - tmp[ii] = (long)((short)(((unsigned short)tmp[ii]) - + ((unsigned short)0x8000))); - } - } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) { - tmp[ii] = (long)((signed char)raw_data[ii]) * 256; - } else { - result = INV_ERROR_FEATURE_NOT_IMPLEMENTED; - } - } - - for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { - data[ii] = ((long)tmp[0] * mtx[3 * ii] + - (long)tmp[1] * mtx[3 * ii + 1] + - (long)tmp[2] * mtx[3 * ii + 2]); - } - - //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]); - return result; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/accel.h b/60xx/mlsdk/mllite/accel.h deleted file mode 100644 index d3fbc6a..0000000 --- a/60xx/mlsdk/mllite/accel.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: accel.h 4580 2011-01-22 03:19:23Z prao $ - * - *******************************************************************************/ - -#ifndef ACCEL_H -#define ACCEL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "accel_legacy.h" -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - - unsigned char inv_accel_present(void); - unsigned char inv_get_slave_addr(void); - inv_error_t inv_get_accel_data(long *data); - unsigned short inv_get_accel_id(void); - -#ifdef __cplusplus -} -#endif -#endif // ACCEL_H diff --git a/60xx/mlsdk/mllite/compass.c b/60xx/mlsdk/mllite/compass.c deleted file mode 100644 index 798cb9f..0000000 --- a/60xx/mlsdk/mllite/compass.c +++ /dev/null @@ -1,579 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup COMPASSDL - * @brief Motion Library - Compass Driver Layer. - * Provides the interface to setup and handle an compass - * connected to either the primary or the seconday I2C interface - * of the gyroscope. - * - * @{ - * @file compass.c - * @brief Compass setup and handling methods. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> -#include <stdlib.h> -#include "compass.h" - -#include "ml.h" -#include "mlinclude.h" -#include "dmpKey.h" -#include "mlFIFO.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mlMathFunc.h" -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-compass" - -#define COMPASS_DEBUG 0 - -/* --------------------- */ -/* - Global Variables. - */ -/* --------------------- */ - -/* --------------------- */ -/* - Static Variables. - */ -/* --------------------- */ - -/* --------------- */ -/* - Prototypes. - */ -/* --------------- */ - -/* -------------- */ -/* - Externs. - */ -/* -------------- */ - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -static float square(float data) -{ - return data * data; -} - -static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise) -{ - int i; - - adap_filter->num = 0; - adap_filter->index = 0; - adap_filter->noise = noise; - adap_filter->len = len; - - for (i = 0; i < adap_filter->len; ++i) { - adap_filter->sequence[i] = 0; - } -} - -static int cmpfloat(const void *p1, const void *p2) -{ - return *(float*)p1 - *(float*)p2; -} - - -static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in) -{ - float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN]; - int i; - - if (adap_filter->len <= 1) { - return in; - } - if (adap_filter->num < adap_filter->len) { - adap_filter->sequence[adap_filter->index++] = in; - adap_filter->num++; - return in; - } - if (adap_filter->len <= adap_filter->index) { - adap_filter->index = 0; - } - adap_filter->sequence[adap_filter->index++] = in; - - avg = 0; - for (i = 0; i < adap_filter->len; i++) { - avg += adap_filter->sequence[i]; - } - avg /= adap_filter->len; - - memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float)); - qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat); - median = sorted[adap_filter->len/2]; - - sum = 0; - for (i = 0; i < adap_filter->len; i++) { - sum += square(avg - adap_filter->sequence[i]); - } - sum /= adap_filter->len; - - if (sum <= adap_filter->noise) { - return median; - } - - return ((in - avg) * (sum - adap_filter->noise) / sum + avg); -} - -static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold) -{ - thresh_filter->threshold = threshold; - thresh_filter->last = 0; -} - -static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in) -{ - if (in < thresh_filter->last - thresh_filter->threshold - || thresh_filter->last + thresh_filter->threshold < in) { - thresh_filter->last = in; - return in; - } - else { - return thresh_filter->last; - } -} - -static int init(yas_filter_handle_t *t) -{ - float noise[] = { - YAS_DEFAULT_FILTER_NOISE, - YAS_DEFAULT_FILTER_NOISE, - YAS_DEFAULT_FILTER_NOISE, - }; - int i; - - if (t == NULL) { - return -1; - } - - for (i = 0; i < 3; i++) { - adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]); - thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH); - } - - return 0; -} - -static int update(yas_filter_handle_t *t, float *input, float *output) -{ - int i; - - if (t == NULL || input == NULL || output == NULL) { - return -1; - } - - for (i = 0; i < 3; i++) { - output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]); - output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]); - } - - return 0; -} - -int yas_filter_init(yas_filter_if_s *f) -{ - if (f == NULL) { - return -1; - } - f->init = init; - f->update = update; - - return 0; -} - -/** - * @brief Used to determine if a compass is - * configured and used by the MPL. - * @return INV_SUCCESS if the compass is present. - */ -unsigned char inv_compass_present(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->compass && - NULL != mldl_cfg->compass->resume && - mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS) - return TRUE; - else - return FALSE; -} - -/** - * @brief Query the compass slave address. - * @return The 7-bit compass slave address. - */ -unsigned char inv_get_compass_slave_addr(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pdata) - return mldl_cfg->pdata->compass.address; - else - return 0; -} - -/** - * @brief Get the ID of the compass in use. - * @return ID of the compass in use. - */ -unsigned short inv_get_compass_id(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->compass) { - return mldl_cfg->compass->id; - } - return ID_INVALID; -} - -/** - * @brief Get a sample of compass data from the device. - * @param data - * the buffer to store the compass raw data for - * X, Y, and Z axes. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_compass_data(long *data) -{ - inv_error_t result; - int ii; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - unsigned char *tmp = inv_obj.compass_raw_data; - - if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); - return INV_ERROR_INVALID_CONFIGURATION; - } - - if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY || - !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { - /*--- read the compass sensor data. - The compass read function may return an INV_ERROR_COMPASS_* errors - when the data is not ready (read/refresh frequency mismatch) or - the internal data sampling timing of the device was not respected. - Returning the error code will make the sensor fusion supervisor - ignore this compass data sample. ---*/ - result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), - tmp); - if (result) { - if (COMPASS_DEBUG) { - MPL_LOGV("inv_mpu_read_compass returned %d\n", result); - } - return result; - } - for (ii = 0; ii < 3; ii++) { - if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian) - data[ii] = - ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; - else - data[ii] = - ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; - } - - inv_obj.compass_overunder = (int)tmp[6]; - - } else { -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - result = inv_get_external_sensor_data(data, 3); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#if defined CONFIG_MPU_SENSORS_MPU6050A2 - { - static unsigned char first = TRUE; - // one-off write to AKM - if (first) { - unsigned char regs[] = { - // beginning Mantis register for one-off slave R/W - MPUREG_I2C_SLV4_ADDR, - // the slave to write to - mldl_cfg->pdata->compass.address, - // the register to write to - /*mldl_cfg->compass->trigger->reg */ 0x0A, - // the value to write - /*mldl_cfg->compass->trigger->value */ 0x01, - // enable the write - 0xC0 - }; - result = - inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr, - ARRAY_SIZE(regs), regs); - first = FALSE; - } else { - unsigned char regs[] = { - MPUREG_I2C_SLV4_CTRL, - 0xC0 - }; - result = - inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr, - ARRAY_SIZE(regs), regs); - } - } -#endif -#else - return INV_ERROR_INVALID_CONFIGURATION; -#endif // CONFIG_MPU_SENSORS_xxxx - } - data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]); - data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]); - data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]); - - return INV_SUCCESS; -} - -/** - * @brief Sets the compass bias. - * @param bias - * Compass bias, length 3. Scale is micro Tesla's * 2^16. - * Frame is mount frame which may be different from body frame. - * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. - */ -inv_error_t inv_set_compass_bias(long *bias) -{ - inv_error_t result = INV_SUCCESS; - long biasC[3]; - struct mldl_cfg *mldlCfg = inv_get_dl_config(); - - inv_obj.compass_bias[0] = bias[0]; - inv_obj.compass_bias[1] = bias[1]; - inv_obj.compass_bias[2] = bias[2]; - - // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame - biasC[0] = - (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) + - inv_obj.init_compass_bias[0] * (1L << 16); - biasC[1] = - (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) + - inv_obj.init_compass_bias[1] * (1L << 16); - biasC[2] = - (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) + - inv_obj.init_compass_bias[2] * (1L << 16); - - if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) { - unsigned char reg[4]; - long biasB[3]; - signed char *orC = mldlCfg->pdata->compass.orientation; - - // Now transform to body frame - biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2]; - biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5]; - biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8]; - - result = - inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4, - inv_int32_to_big8(biasB[0], reg)); - result = - inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4, - inv_int32_to_big8(biasB[1], reg)); - result = - inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4, - inv_int32_to_big8(biasB[2], reg)); - } - return result; -} - -/** - * @brief Write a single register on the compass slave device, regardless - * of the bus it is connected to and the MPU's configuration. - * @param reg - * the register to write to on the slave compass device. - * @param val - * the value to write. - * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. - */ -inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val) -{ - struct ext_slave_config config; - unsigned char data[2]; - inv_error_t result; - - data[0] = reg; - data[1] = val; - - config.key = MPU_SLAVE_WRITE_REGISTERS; - config.len = 2; - config.apply = TRUE; - config.data = data; - - result = inv_mpu_config_compass(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief Read values from the compass slave device registers, regardless - * of the bus it is connected to and the MPU's configuration. - * @param reg - * the register to read from on the slave compass device. - * @param val - * a buffer of 3 elements to store the values read from the - * compass device. - * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. - */ -inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val) -{ - struct ext_slave_config config; - unsigned char data[2]; - inv_error_t result; - - data[0] = reg; - - config.key = MPU_SLAVE_READ_REGISTERS; - config.len = 2; - config.apply = TRUE; - config.data = data; - - result = inv_mpu_get_compass_config(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - *val = data[1]; - return result; -} - -/** - * @brief Read values from the compass slave device scale registers, regardless - * of the bus it is connected to and the MPU's configuration. - * @param reg - * the register to read from on the slave compass device. - * @param val - * a buffer of 3 elements to store the values read from the - * compass device. - * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. - */ -inv_error_t inv_compass_read_scale(long *val) -{ - struct ext_slave_config config; - unsigned char data[3]; - inv_error_t result; - - config.key = MPU_SLAVE_READ_SCALE; - config.len = 3; - config.apply = TRUE; - config.data = data; - - result = inv_mpu_get_compass_config(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - val[0] = ((data[0] - 128) << 22) + (1L << 30); - val[1] = ((data[1] - 128) << 22) + (1L << 30); - val[2] = ((data[2] - 128) << 22) + (1L << 30); - return result; -} - -inv_error_t inv_set_compass_offset(void) -{ - struct ext_slave_config config; - unsigned char data[3]; - inv_error_t result; - - config.key = MPU_SLAVE_OFFSET_VALS; - config.len = 3; - config.apply = TRUE; - config.data = data; - - if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) { - /* push stored values */ - data[0] = (char)inv_obj.compass_offsets[0]; - data[1] = (char)inv_obj.compass_offsets[1]; - data[2] = (char)inv_obj.compass_offsets[2]; - MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]); - result = inv_mpu_config_compass(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - } else { - /* compute new values and store them */ - result = inv_mpu_get_compass_config(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]); - if(result == INV_SUCCESS) { - inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1; - inv_obj.compass_offsets[0] = data[0]; - inv_obj.compass_offsets[1] = data[1]; - inv_obj.compass_offsets[2] = data[2]; - } - } - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -inv_error_t inv_compass_check_range(void) -{ - struct ext_slave_config config; - unsigned char data[3]; - inv_error_t result; - - config.key = MPU_SLAVE_RANGE_CHECK; - config.len = 3; - config.apply = TRUE; - config.data = data; - - result = inv_mpu_get_compass_config(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if(data[0] || data[1] || data[2]) { - /* some value clipped */ - return INV_ERROR_COMPASS_DATA_ERROR; - } - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/compass.h b/60xx/mlsdk/mllite/compass.h deleted file mode 100644 index f0bdb58..0000000 --- a/60xx/mlsdk/mllite/compass.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: compass.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#ifndef COMPASS_H -#define COMPASS_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "compass_legacy.h" -#endif - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -#define YAS_MAX_FILTER_LEN (20) -#define YAS_DEFAULT_FILTER_LEN (20) -#define YAS_DEFAULT_FILTER_THRESH (300) /* 300 nT */ -#define YAS_DEFAULT_FILTER_NOISE (2000 * 2000) /* standard deviation 2000 nT */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - -struct yas_adaptive_filter { - int num; - int index; - int len; - float noise; - float sequence[YAS_MAX_FILTER_LEN]; -}; - -struct yas_thresh_filter { - float threshold; - float last; -}; - -typedef struct { - struct yas_adaptive_filter adap_filter[3]; - struct yas_thresh_filter thresh_filter[3]; -} yas_filter_handle_t; - -typedef struct { - int (*init)(yas_filter_handle_t *t); - int (*update)(yas_filter_handle_t *t, float *input, float *output); -} yas_filter_if_s; - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - - unsigned char inv_compass_present(void); - unsigned char inv_get_compass_slave_addr(void); - inv_error_t inv_get_compass_data(long *data); - inv_error_t inv_set_compass_bias(long *bias); - unsigned short inv_get_compass_id(void); - inv_error_t inv_set_compass_offset(void); - inv_error_t inv_compass_check_range(void); - inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val); - inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val); - inv_error_t inv_compass_read_scale(long *val); - - int yas_filter_init(yas_filter_if_s *f); - -#ifdef __cplusplus -} -#endif -#endif // COMPASS_H diff --git a/60xx/mlsdk/mllite/dmpDefault.c b/60xx/mlsdk/mllite/dmpDefault.c deleted file mode 100644 index 6620d14..0000000 --- a/60xx/mlsdk/mllite/dmpDefault.c +++ /dev/null @@ -1,417 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/***************************************************************************** * - * $Id: dmpDefault.c 5627 2011-06-10 22:34:18Z nroyer $ - ******************************************************************************/ - -/* WARNING: autogenerated code, do not modify */ -/** - * @defgroup DMPDEFAULT - * @brief Data and configuration for MLDmpDefaultOpen. - * - * @{ - * @file inv_setup_dmp.c - * @brief Data and configuration for MLDmpDefaultOpen. - */ - -#include "mltypes.h" -#include "dmpDefault.h" -#include "dmpKey.h" -#include "dmpmap.h" -#include "ml.h" -#include "mpu.h" -#include "mldl.h" -#include "mldl_cfg.h" - -#define CFG_25 703 -#define CFG_24 699 -#define CFG_26 707 -#define CFG_21 802 -#define CFG_20 645 -#define CFG_23 814 -#define CFG_TAP4 808 -#define CFG_TAP5 809 -#define CFG_TAP6 810 -#define CFG_1 783 -#define CFG_TAP0 802 -#define CFG_TAP1 804 -#define CFG_TAP2 805 -#define CFG_TAP3 806 -#define FCFG_AZ 878 -#define CFG_ORIENT_IRQ_1 715 -#define CFG_ORIENT_IRQ_2 738 -#define CFG_ORIENT_IRQ_3 743 -#define CFG_TAP_QUANTIZE 647 -#define FCFG_3 936 -#define CFG_TAP_CLEAR_STICKY 817 -#define FCFG_1 868 -#define CFG_ACCEL_FILTER 968 -#define FCFG_2 872 -#define CFG_3D 521 -#define CFG_3B 517 -#define CFG_3C 519 -#define FCFG_5 942 -#define FCFG_4 857 -#define FCFG_FSCALE 877 -#define CFG_TAP_JERK 639 -#define FCFG_6 996 -#define CFG_12 797 -#define FCFG_7 930 -#define CFG_14 790 -#define CFG_15 790 -#define CFG_16 815 -#define CFG_18 551 -#define CFG_6 823 -#define CFG_7 564 -#define CFG_4 526 -#define CFG_5 749 -#define CFG_3 515 -#define CFG_GYRO_SOURCE 777 -#define CFG_8 772 -#define CFG_9 778 -#define CFG_ORIENT_2 733 -#define CFG_ORIENT_1 713 -#define FCFG_ACCEL_INPUT 904 -#define CFG_TAP7 811 -#define CFG_TAP_SAVE_ACCB 687 -#define FCFG_ACCEL_INIT 831 - - -#define D_0_22 (22) -#define D_0_24 (24) -#define D_0_36 (36) -#define D_0_52 (52) -#define D_0_96 (96) -#define D_0_104 (104) -#define D_0_108 (108) -#define D_0_163 (163) -#define D_0_188 (188) -#define D_0_192 (192) -#define D_0_224 (224) -#define D_0_228 (228) -#define D_0_232 (232) -#define D_0_236 (236) - -#define D_1_2 (256 + 2) -#define D_1_4 (256 + 4) -#define D_1_8 (256 + 8) -#define D_1_10 (256 + 10) -#define D_1_24 (256 + 24) -#define D_1_28 (256 + 28) -#define D_1_92 (256 + 92) -#define D_1_96 (256 + 96) -#define D_1_98 (256 + 98) -#define D_1_106 (256 + 106) -#define D_1_108 (256 + 108) -#define D_1_112 (256 + 112) -#define D_1_128 (256 + 144) -#define D_1_152 (256 + 12) -#define D_1_168 (256 + 168) -#define D_1_175 (256 + 175) -#define D_1_178 (256 + 178) -#define D_1_236 (256 + 236) -#define D_1_244 (256 + 244) - - -static const tKeyLabel dmpTConfig[] = { - {KEY_CFG_25, CFG_25}, - {KEY_CFG_24, CFG_24}, - {KEY_CFG_26, CFG_26}, - {KEY_CFG_21, CFG_21}, - {KEY_CFG_20, CFG_20}, - {KEY_CFG_23, CFG_23}, - {KEY_CFG_TAP4, CFG_TAP4}, - {KEY_CFG_TAP5, CFG_TAP5}, - {KEY_CFG_TAP6, CFG_TAP6}, - {KEY_CFG_1, CFG_1}, - {KEY_CFG_TAP0, CFG_TAP0}, - {KEY_CFG_TAP1, CFG_TAP1}, - {KEY_CFG_TAP2, CFG_TAP2}, - {KEY_CFG_TAP3, CFG_TAP3}, - {KEY_FCFG_AZ, FCFG_AZ}, - {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1}, - {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2}, - {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3}, - {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE}, - {KEY_FCFG_3, FCFG_3}, - {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY}, - {KEY_FCFG_1, FCFG_1}, - //{KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER}, - {KEY_FCFG_2, FCFG_2}, - {KEY_CFG_3D, CFG_3D}, - {KEY_CFG_3B, CFG_3B}, - {KEY_CFG_3C, CFG_3C}, - {KEY_FCFG_5, FCFG_5}, - {KEY_FCFG_4, FCFG_4}, - {KEY_FCFG_FSCALE, FCFG_FSCALE}, - {KEY_CFG_TAP_JERK, CFG_TAP_JERK}, - {KEY_FCFG_6, FCFG_6}, - {KEY_CFG_12, CFG_12}, - {KEY_FCFG_7, FCFG_7}, - {KEY_CFG_14, CFG_14}, - {KEY_CFG_15, CFG_15}, - {KEY_CFG_16, CFG_16}, - {KEY_CFG_18, CFG_18}, - {KEY_CFG_6, CFG_6}, - {KEY_CFG_7, CFG_7}, - {KEY_CFG_4, CFG_4}, - {KEY_CFG_5, CFG_5}, - {KEY_CFG_3, CFG_3}, - {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE}, - {KEY_CFG_8, CFG_8}, - {KEY_CFG_9, CFG_9}, - {KEY_CFG_ORIENT_2, CFG_ORIENT_2}, - {KEY_CFG_ORIENT_1, CFG_ORIENT_1}, - {KEY_FCFG_ACCEL_INPUT, FCFG_ACCEL_INPUT}, - {KEY_CFG_TAP7, CFG_TAP7}, - {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB}, - {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT}, - - {KEY_D_0_22, D_0_22}, - {KEY_D_0_24, D_0_24}, - {KEY_D_0_36, D_0_36}, - {KEY_D_0_52, D_0_52}, - {KEY_D_0_96, D_0_96}, - {KEY_D_0_104, D_0_104}, - {KEY_D_0_108, D_0_108}, - {KEY_D_0_163, D_0_163}, - {KEY_D_0_188, D_0_188}, - {KEY_D_0_192, D_0_192}, - {KEY_D_0_224, D_0_224}, - {KEY_D_0_228, D_0_228}, - {KEY_D_0_232, D_0_232}, - {KEY_D_0_236, D_0_236}, - - {KEY_DMP_PREVPTAT, DMP_PREVPTAT}, - {KEY_D_1_2, D_1_2}, - {KEY_D_1_4, D_1_4}, - {KEY_D_1_8, D_1_8}, - {KEY_D_1_10, D_1_10}, - {KEY_D_1_24, D_1_24}, - {KEY_D_1_28, D_1_28}, - {KEY_D_1_92, D_1_92}, - {KEY_D_1_96, D_1_96}, - {KEY_D_1_98, D_1_98}, - {KEY_D_1_106, D_1_106}, - {KEY_D_1_108, D_1_108}, - {KEY_D_1_112, D_1_112}, - {KEY_D_1_128, D_1_128}, - {KEY_D_1_152, D_1_152}, - {KEY_D_1_168, D_1_168}, - {KEY_D_1_175, D_1_175}, - {KEY_D_1_178, D_1_178}, - {KEY_D_1_236, D_1_236}, - {KEY_D_1_244, D_1_244}, - - {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN}, - {KEY_DMP_TAP_THR_X, DMP_TAP_THX}, - {KEY_DMP_TAP_THR_Y, DMP_TAP_THY}, - {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ}, - {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y}, - {KEY_DMP_SH_TH_X, DMP_SH_TH_X}, - {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z}, - {KEY_DMP_ORIENT, DMP_ORIENT} -}; - -#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) -static const unsigned short sConfig = 0x013f; -#define SCD (1024) -static const unsigned char dmpMemory[SCD] = { - 0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x5a, 0xd6, 0x96, 0x06, 0x3f, 0xa3, 0x00, 0x00, - 0x20, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x77, 0x8e, 0x00, 0x01, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01, - 0x02, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x03, 0x06, 0x00, 0x00, 0x05, 0x01, 0xe9, 0xa2, 0x0f, - 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x3e, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f, - 0xfc, 0xba, 0xfa, 0x00, 0x01, 0x00, 0x80, 0x00, 0x02, 0x01, 0x80, 0x00, 0x03, 0x02, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f, - 0x00, 0x7d, 0x32, 0xba, 0x00, 0x0a, 0x1e, 0xd1, 0x00, 0x3a, 0xe8, 0x25, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0xd7, 0x96, 0x08, 0xff, 0xb3, 0x39, 0xf5, 0xfe, 0x11, 0x1b, 0x62, 0xfb, 0xf4, 0xb4, 0x52, - 0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc, - 0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4, - 0x0d, 0x68, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10, - - 0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8, - 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x12, 0x82, 0x2d, 0x90, - 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0xff, 0xff, 0x00, 0x05, 0x02, 0x00, 0x00, 0x0c, - 0x00, 0x03, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0xff, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, - 0xff, 0xec, 0x3f, 0xc8, 0xff, 0xee, 0x00, 0x00, 0xff, 0xfe, 0x40, 0x00, 0xff, 0xff, 0xff, 0xc8, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xff, 0xff, 0xff, 0xff, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 0x03, 0x65, 0x00, 0x00, 0x00, 0x99, 0x00, 0x00, 0x02, 0xf5, - - 0x9e, 0xc5, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, 0xc1, 0x83, 0x06, 0x26, - 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xc4, 0xad, 0x00, 0x2c, 0x54, 0x7c, 0xf9, 0xc5, 0xa3, - 0xc1, 0xc3, 0x8f, 0x96, 0x19, 0xa6, 0x81, 0xda, 0x0c, 0xd9, 0x2e, 0xd8, 0xa3, 0x86, 0x31, 0x81, - 0xa6, 0xd9, 0x30, 0x26, 0xd8, 0xd8, 0xfa, 0xc1, 0x8c, 0xc2, 0x99, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, - 0x81, 0x91, 0xac, 0x38, 0xad, 0x3a, 0xc3, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, - 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9, - 0x04, 0xae, 0xd8, 0x79, 0xd9, 0x04, 0xd8, 0x81, 0xfb, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81, - 0xad, 0xd9, 0x01, 0xd8, 0xfa, 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad, - 0xad, 0xad, 0xad, 0xfb, 0x2a, 0xd8, 0xd8, 0xf9, 0xc0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xfb, - 0xac, 0x2e, 0x2e, 0xf9, 0xc1, 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30, - 0x18, 0xa8, 0x98, 0x81, 0x28, 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xfa, - 0xc0, 0x89, 0xac, 0x91, 0x2c, 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8, - 0xd8, 0x79, 0xd9, 0xd8, 0xd8, 0xf9, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9, - 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xc1, 0x83, 0x93, 0x35, 0x3d, 0x80, - 0x25, 0xda, 0xd8, 0xd8, 0x85, 0x69, 0xda, 0xd8, 0xd8, 0xf9, 0xc2, 0x93, 0x81, 0xa3, 0x28, 0x34, - 0x3c, 0xfb, 0x91, 0xab, 0x8b, 0x18, 0xa3, 0x09, 0xd9, 0xab, 0x97, 0x0a, 0x91, 0x3c, 0xc0, 0x87, - - 0x9c, 0xc5, 0xa3, 0xdd, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x95, 0xf9, 0xa3, 0xa3, 0xa3, 0x9d, 0xf9, - 0xa3, 0xa3, 0xa3, 0xa3, 0xf9, 0x90, 0xa3, 0xa3, 0xa3, 0xa3, 0x91, 0xc3, 0x99, 0xf9, 0xa3, 0xa3, - 0xa3, 0x98, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xfb, 0x9b, 0xa3, 0xa3, - 0xdc, 0xc5, 0xa7, 0xf9, 0x26, 0x26, 0x26, 0xd8, 0xd8, 0xff, 0xd8, 0xd8, 0xd8, 0xd8, 0xd8, 0xc1, - 0xc2, 0xc4, 0x81, 0xa0, 0x90, 0xfa, 0x2c, 0x80, 0x74, 0xfb, 0x70, 0xfa, 0x7c, 0xc0, 0x86, 0x98, - 0xa8, 0xf9, 0xc9, 0x88, 0xa1, 0xfa, 0x0e, 0x97, 0x80, 0xf9, 0xa9, 0x2e, 0x2e, 0x2e, 0xaa, 0x2e, - 0x2e, 0x2e, 0xfa, 0xaa, 0xc9, 0x2c, 0xcb, 0xa9, 0x4c, 0xcd, 0x6c, 0xf9, 0x89, 0xa5, 0xca, 0xcd, - 0xcf, 0xc3, 0x9e, 0xa9, 0x3e, 0x5e, 0x7e, 0x85, 0xa5, 0x1a, 0x3e, 0x5e, 0xc2, 0xa5, 0x99, 0xfb, - 0x08, 0x34, 0x5c, 0xf9, 0xa9, 0xc9, 0xcb, 0xcd, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, - 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0xa9, - 0xf9, 0x89, 0x26, 0x46, 0x66, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xaa, 0x98, 0x82, 0x87, 0x2d, - 0x35, 0x3d, 0xc5, 0xa3, 0xc2, 0xc1, 0x97, 0x80, 0x4a, 0x4e, 0x4e, 0xa3, 0xfa, 0x48, 0xcd, 0xc9, - 0xf9, 0xc4, 0xa9, 0x99, 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, 0xc3, 0x93, - 0xa3, 0x0e, 0x16, 0x1e, 0xa9, 0x2c, 0x54, 0x7c, 0xc0, 0xc2, 0x83, 0x97, 0xaf, 0x08, 0xc4, 0xa8, - 0x11, 0xc1, 0x8f, 0xc5, 0xaf, 0x98, 0xf8, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf9, 0xa3, 0x29, - 0x55, 0x7d, 0xaf, 0x83, 0xc3, 0x93, 0xaf, 0xf8, 0x00, 0x28, 0x50, 0xc4, 0xc2, 0xc0, 0xf9, 0x97, -}; -static tKeyLabel keys[NUM_KEYS]; - -static unsigned short inv_setup_dmpGetAddress(unsigned short key) -{ - static int isSorted = 0; - if ( !isSorted ) { - int kk; - for (kk=0; kk<NUM_KEYS; ++kk) { - keys[ kk ].addr = 0xffff; - keys[ kk ].key = kk; - } - for (kk=0; kk<NUM_LOCAL_KEYS; ++kk) { - keys[ dmpTConfig[kk].key ].addr = dmpTConfig[kk].addr; - } - isSorted = 1; - } - if ( key >= NUM_KEYS ) - return 0xffff; - return keys[ key ].addr; -} - - -/** - * @brief - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_setup_dmp(void) -{ - inv_error_t result; - inv_set_get_address( inv_setup_dmpGetAddress ); - - result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_full_scale(2000.f); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_load_dmp(dmpMemory, SCD, sConfig); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_ignore_system_suspend(FALSE); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (inv_accel_present()) - { - struct ext_slave_config config; - long odr; - config.key = MPU_SLAVE_CONFIG_ODR_SUSPEND; - config.len = sizeof(long); - config.apply = FALSE; - config.data = &odr; - - odr = 0; - result = inv_mpu_config_accel(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), - &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - config.key = MPU_SLAVE_CONFIG_ODR_RESUME; - odr = 200000; - result = inv_mpu_config_accel(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), - &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - config.key = MPU_SLAVE_CONFIG_IRQ_SUSPEND; - odr = MPU_SLAVE_IRQ_TYPE_NONE; - result = inv_mpu_config_accel(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), - &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; - odr = MPU_SLAVE_IRQ_TYPE_NONE; - result = inv_mpu_config_accel(inv_get_dl_config(), - inv_get_serial_handle(), - inv_get_serial_handle(), - &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - } - - return result; -} -/** - * @} - */ - diff --git a/60xx/mlsdk/mllite/dmpDefault.h b/60xx/mlsdk/mllite/dmpDefault.h deleted file mode 100644 index 0670977..0000000 --- a/60xx/mlsdk/mllite/dmpDefault.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef ML_DMPDEFAULT_H__ -#define ML_DMPDEFAULT_H__ - -/** - * @defgroup DEFAULT - * @brief Default DMP assembly listing header file - * - * @{ - * @file inv_setup_dmp.c - * @brief Default DMP assembly listing header file - */ - - -#ifdef __cplusplus -extern "C" -{ -#endif - inv_error_t inv_setup_dmp(void); - -#ifdef __cplusplus -} -#endif - - -#endif // ML_DMPDEFAULT_H__ diff --git a/60xx/mlsdk/mllite/dmpDefaultMantis.c b/60xx/mlsdk/mllite/dmpDefaultMantis.c deleted file mode 100644 index f35aaca..0000000 --- a/60xx/mlsdk/mllite/dmpDefaultMantis.c +++ /dev/null @@ -1,467 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/* WARNING: autogenerated code, do not modify */ -/** - * @defgroup DMPDEFAULT - * @brief - * - * @{ - * @file inv_setup_dmpMantis.c - * @brief - * - * - */ - -#include "mltypes.h" -#include "dmpDefault.h" -#include "dmpKey.h" -#include "dmpmap.h" -#include "mldl.h" - -#define CFG_25 1786 -#define CFG_24 1782 -#define CFG_26 1790 -#define CFG_21 1899 -#define CFG_20 1728 -#define CFG_23 1911 -#define CFG_TAP4 1905 -#define CFG_TAP5 1906 -#define CFG_TAP6 1907 -#define CFG_1 1865 -#define CFG_TAP0 1899 -#define CFG_TAP1 1901 -#define CFG_TAP2 1902 -#define CFG_TAP_SAVE_ACCB 1770 -#define CFG_ORIENT_IRQ_1 1798 -#define CFG_ORIENT_IRQ_2 1821 -#define CFG_ORIENT_IRQ_3 1826 -#define FCFG_MAG_VAL 774 -#define CFG_TAP_QUANTIZE 1730 -#define FCFG_3 936 -#define FCFG_1 891 -#define CFG_ACCEL_FILTER 1076 -#define FCFG_2 895 -#define CFG_3D 1629 -#define FCFG_7 902 -#define CFG_3C 1627 -#define FCFG_5 1030 -#define FCFG_4 880 -#define CFG_3B 1625 -#define CFG_TAP_JERK 1722 -#define FCFG_6 954 -#define CFG_12 1894 -#define CFG_TAP7 1908 -#define CFG_14 1871 -#define CFG_15 1876 -#define CFG_16 1912 -#define CFG_TAP_CLEAR_STICKY 1914 -#define CFG_6 1920 -#define CFG_7 1014 -#define CFG_4 1634 -#define CFG_5 1831 -#define CFG_3 1623 -#define CFG_GYRO_SOURCE 1859 -#define CFG_TAP3 1903 -#define CFG_EXTERNAL 1884 -#define CFG_8 1854 -#define CFG_9 1860 -#define CFG_ORIENT_2 1816 -#define CFG_ORIENT_1 1796 -#define CFG_MOTION_BIAS 1023 -#define FCFG_MAG_MOV 791 -#define TEMPLABEL 1491 -#define FCFG_ACCEL_INIT 847 - - -#define D_0_22 (22+512) -#define D_0_24 (24+512) - -#define D_0_36 (36) -#define D_0_52 (52) -#define D_0_96 (96) -#define D_0_104 (104) -#define D_0_108 (108) -#define D_0_163 (163) -#define D_0_188 (188) -#define D_0_192 (192) -#define D_0_224 (224) -#define D_0_228 (228) -#define D_0_232 (232) -#define D_0_236 (236) - -#define D_1_2 (256 + 2) -#define D_1_4 (256 + 4) -#define D_1_8 (256 + 8) -#define D_1_10 (256 + 10) -#define D_1_24 (256 + 24) -#define D_1_28 (256 + 28) -#define D_1_92 (256 + 92) -#define D_1_96 (256 + 96) -#define D_1_98 (256 + 98) -#define D_1_106 (256 + 106) -#define D_1_108 (256 + 108) -#define D_1_112 (256 + 112) -#define D_1_128 (256 + 144) -#define D_1_152 (256 + 12) -#define D_1_168 (256 + 168) -#define D_1_175 (256 + 175) -#define D_1_178 (256 + 178) -#define D_1_236 (256 + 236) -#define D_1_244 (256 + 244) -#define D_2_12 (512+12) -#define D_2_96 (512+96) -#define D_2_108 (512+108) -#define D_2_244 (512+244) -#define D_2_248 (512+248) -#define D_2_252 (512+252) - -#define CPASS_BIAS_X (35*16+4) -#define CPASS_BIAS_Y (35*16+8) -#define CPASS_BIAS_Z (35*16+12) -#define CPASS_MTX_00 (36*16) -#define CPASS_MTX_01 (36*16+4) -#define CPASS_MTX_02 (36*16+8) -#define CPASS_MTX_10 (36*16+12) -#define CPASS_MTX_11 (37*16) -#define CPASS_MTX_12 (37*16+4) -#define CPASS_MTX_20 (37*16+8) -#define CPASS_MTX_21 (37*16+12) -#define CPASS_MTX_22 (43*16+12) -#define D_ACT0 (40*16) -#define D_ACSX (40*16+4) -#define D_ACSY (40*16+8) -#define D_ACSZ (40*16+12) - -static const tKeyLabel dmpTConfig[] = { - {KEY_CFG_25, CFG_25}, - {KEY_CFG_24, CFG_24}, - {KEY_CFG_26, CFG_26}, - {KEY_CFG_21, CFG_21}, - {KEY_CFG_20, CFG_20}, - {KEY_CFG_23, CFG_23}, - {KEY_CFG_TAP4, CFG_TAP4}, - {KEY_CFG_TAP5, CFG_TAP5}, - {KEY_CFG_TAP6, CFG_TAP6}, - {KEY_CFG_1, CFG_1}, - {KEY_CFG_TAP0, CFG_TAP0}, - {KEY_CFG_TAP1, CFG_TAP1}, - {KEY_CFG_TAP2, CFG_TAP2}, - {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB}, - {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1}, - {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2}, - {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3}, - {KEY_FCFG_MAG_VAL, FCFG_MAG_VAL}, - {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE}, - {KEY_FCFG_3, FCFG_3}, - {KEY_FCFG_1, FCFG_1}, -// {KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER}, - {KEY_FCFG_2, FCFG_2}, - {KEY_CFG_3D, CFG_3D}, - {KEY_FCFG_7, FCFG_7}, - {KEY_CFG_3C, CFG_3C}, - {KEY_FCFG_5, FCFG_5}, - {KEY_FCFG_4, FCFG_4}, - {KEY_CFG_3B, CFG_3B}, - {KEY_CFG_TAP_JERK, CFG_TAP_JERK}, - {KEY_FCFG_6, FCFG_6}, - {KEY_CFG_12, CFG_12}, - {KEY_CFG_TAP7, CFG_TAP7}, - {KEY_CFG_14, CFG_14}, - {KEY_CFG_15, CFG_15}, - {KEY_CFG_16, CFG_16}, - {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY}, - {KEY_CFG_6, CFG_6}, - {KEY_CFG_7, CFG_7}, - {KEY_CFG_4, CFG_4}, - {KEY_CFG_5, CFG_5}, - {KEY_CFG_3, CFG_3}, - {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE}, - {KEY_CFG_TAP3, CFG_TAP3}, - {KEY_CFG_EXTERNAL, CFG_EXTERNAL}, - {KEY_CFG_8, CFG_8}, - {KEY_CFG_9, CFG_9}, - {KEY_CFG_ORIENT_2, CFG_ORIENT_2}, - {KEY_CFG_ORIENT_1, CFG_ORIENT_1}, - {KEY_CFG_MOTION_BIAS, CFG_MOTION_BIAS}, - {KEY_FCFG_MAG_MOV, FCFG_MAG_MOV}, -// {KEY_TEMPLABEL, TEMPLABEL}, - {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT}, - {KEY_D_0_22, D_0_22}, - {KEY_D_0_24, D_0_24}, - {KEY_D_0_36, D_0_36}, - {KEY_D_0_52, D_0_52}, - {KEY_D_0_96, D_0_96}, - {KEY_D_0_104, D_0_104}, - {KEY_D_0_108, D_0_108}, - {KEY_D_0_163, D_0_163}, - {KEY_D_0_188, D_0_188}, - {KEY_D_0_192, D_0_192}, - {KEY_D_0_224, D_0_224}, - {KEY_D_0_228, D_0_228}, - {KEY_D_0_232, D_0_232}, - {KEY_D_0_236, D_0_236}, - - {KEY_DMP_PREVPTAT, DMP_PREVPTAT}, - {KEY_D_1_2, D_1_2}, - {KEY_D_1_4, D_1_4}, - {KEY_D_1_8, D_1_8}, - {KEY_D_1_10, D_1_10}, - {KEY_D_1_24, D_1_24}, - {KEY_D_1_28, D_1_28}, - {KEY_D_1_92, D_1_92}, - {KEY_D_1_96, D_1_96}, - {KEY_D_1_98, D_1_98}, - {KEY_D_1_106, D_1_106}, - {KEY_D_1_108, D_1_108}, - {KEY_D_1_112, D_1_112}, - {KEY_D_1_128, D_1_128}, - {KEY_D_1_152, D_1_152}, - {KEY_D_1_168, D_1_168}, - {KEY_D_1_175, D_1_175}, - {KEY_D_1_178, D_1_178}, - {KEY_D_1_236, D_1_236}, - {KEY_D_1_244, D_1_244}, - - {KEY_D_2_12, D_2_12}, - {KEY_D_2_96, D_2_96}, - {KEY_D_2_108, D_2_108}, - {KEY_D_2_244, D_2_244}, - {KEY_D_2_248, D_2_248}, - {KEY_D_2_252, D_2_252}, - - {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN}, - {KEY_DMP_TAP_THR_X, DMP_TAP_THX}, - {KEY_DMP_TAP_THR_Y, DMP_TAP_THY}, - {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ}, - {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y}, - {KEY_DMP_SH_TH_X, DMP_SH_TH_X}, - {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z}, - {KEY_DMP_ORIENT, DMP_ORIENT}, - - {KEY_CPASS_BIAS_X, CPASS_BIAS_X}, - {KEY_CPASS_BIAS_Y, CPASS_BIAS_Y}, - {KEY_CPASS_BIAS_Z, CPASS_BIAS_Z}, - {KEY_CPASS_MTX_00, CPASS_MTX_00}, - {KEY_CPASS_MTX_01, CPASS_MTX_01}, - {KEY_CPASS_MTX_02, CPASS_MTX_02}, - {KEY_CPASS_MTX_10, CPASS_MTX_10}, - {KEY_CPASS_MTX_11, CPASS_MTX_11}, - {KEY_CPASS_MTX_12, CPASS_MTX_12}, - {KEY_CPASS_MTX_20, CPASS_MTX_20}, - {KEY_CPASS_MTX_21, CPASS_MTX_21}, - {KEY_CPASS_MTX_22, CPASS_MTX_22}, - {KEY_D_ACT0, D_ACT0}, - {KEY_D_ACSX, D_ACSX}, - {KEY_D_ACSY, D_ACSY}, - {KEY_D_ACSZ, D_ACSZ} -}; -#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) - -#define DMP_CODE_SIZE 1923 - -static const unsigned char dmpMemory[DMP_CODE_SIZE] = { - 0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xff, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x2a, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xfd, 0x87, 0x26, 0x50, 0xfd, 0x80, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6f, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5e, 0xc0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc, - 0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4, - 0xff, 0xe6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10, - 0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8, - 0xff, 0xd0, 0x9a, 0xbe, 0x00, 0x00, 0x10, 0xa9, 0xff, 0xf4, 0x1e, 0xb2, 0x00, 0xce, 0xbb, 0xf7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0c, - 0xff, 0xc2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xcf, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3f, 0x68, 0xb6, 0x79, 0x35, 0x28, 0xbc, 0xc6, 0x7e, 0xd1, 0x6c, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xff, 0xef, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - 0xd8, 0xdc, 0xba, 0xa2, 0xf1, 0xde, 0xb2, 0xb8, 0xb4, 0xa8, 0x81, 0x91, 0xf7, 0x4a, 0x90, 0x7f, - 0x91, 0x6a, 0xf3, 0xf9, 0xdb, 0xa8, 0xf9, 0xb0, 0xba, 0xa0, 0x80, 0xf2, 0xce, 0x81, 0xf3, 0xc2, - 0xf1, 0xc1, 0xf2, 0xc3, 0xf3, 0xcc, 0xa2, 0xb2, 0x80, 0xf1, 0xc6, 0xd8, 0x80, 0xba, 0xa7, 0xdf, - 0xdf, 0xdf, 0xf2, 0xa7, 0xc3, 0xcb, 0xc5, 0xb6, 0xf0, 0x87, 0xa2, 0x94, 0x24, 0x48, 0x70, 0x3c, - 0x95, 0x40, 0x68, 0x34, 0x58, 0x9b, 0x78, 0xa2, 0xf1, 0x83, 0x92, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, - 0xb4, 0xb8, 0xa1, 0xd0, 0x91, 0x80, 0xf2, 0x70, 0xf3, 0x70, 0xf2, 0x7c, 0x80, 0xa8, 0xf1, 0x01, - 0xb0, 0x98, 0x87, 0xd9, 0x43, 0xd8, 0x86, 0xc9, 0x88, 0xba, 0xa1, 0xf2, 0x0e, 0xb8, 0x97, 0x80, - 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0xc1, - 0xc9, 0xc3, 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, - 0xca, 0xf1, 0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, - 0x99, 0x2c, 0x54, 0x7c, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xb9, 0xaf, 0xb4, 0xb0, - 0x83, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb1, 0x8f, 0x98, 0xb9, 0xaf, 0xf0, 0x24, 0x08, 0x44, 0x10, - 0x64, 0x18, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xaf, 0x83, 0xb5, 0x93, 0xaf, 0xf0, 0x00, 0x28, 0x50, - 0xf1, 0xa3, 0x86, 0x9f, 0x61, 0xa6, 0xda, 0xde, 0xdf, 0xd9, 0xfa, 0xa3, 0x86, 0x96, 0xdb, 0x31, - 0xa6, 0xd9, 0xf8, 0xdf, 0xba, 0xa6, 0x8f, 0xc2, 0xc5, 0xc7, 0xb2, 0x8c, 0xc1, 0xb8, 0xa2, 0xdf, - 0xdf, 0xdf, 0xa3, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb2, 0x86, 0xb4, 0x98, 0x0d, - 0x35, 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, - 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xb9, - 0xa3, 0xde, 0xdf, 0xdf, 0xa3, 0xb1, 0x80, 0xf2, 0xc4, 0xcd, 0xc9, 0xf1, 0xb8, 0xa9, 0xb4, 0x99, - 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0xb5, 0x93, 0xa3, 0x0e, 0x16, 0x1e, - 0xa9, 0x2c, 0x54, 0x7c, 0xb8, 0xb4, 0xb0, 0xf1, 0x97, 0x83, 0xa8, 0x11, 0x84, 0xa5, 0x09, 0x98, - 0xa3, 0x83, 0xf0, 0xda, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xd8, 0xf1, 0xa5, 0x29, 0x55, 0x7d, - 0xa5, 0x85, 0x95, 0x02, 0x1a, 0x2e, 0x3a, 0x56, 0x5a, 0x40, 0x48, 0xf9, 0xf3, 0xa3, 0xd9, 0xf8, - 0xf0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xa8, 0xf1, 0x11, 0xf0, 0x98, - 0xa2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xda, 0xf3, 0xde, 0xd8, 0x83, 0xa5, 0x94, 0x01, 0xd9, - 0xa3, 0x02, 0xf1, 0xa2, 0xc3, 0xc5, 0xc7, 0xd8, 0xf1, 0x84, 0x92, 0xa2, 0x4d, 0xda, 0x2a, 0xd8, - 0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, - 0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0x93, 0xa3, 0x4d, 0xda, 0x2a, 0xd8, - 0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, - 0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0xa8, 0x8a, 0x9a, 0xf0, 0x28, 0x50, - 0x78, 0x9e, 0xf3, 0x88, 0x18, 0xf1, 0x9f, 0x1d, 0x98, 0xa8, 0xd9, 0x08, 0xd8, 0xc8, 0x9f, 0x12, - 0x9e, 0xf3, 0x15, 0xa8, 0xda, 0x12, 0x10, 0xd8, 0xf1, 0xaf, 0xc8, 0x97, 0x87, 0x34, 0xb5, 0xb9, - 0x94, 0xa4, 0x21, 0xf3, 0xd9, 0x22, 0xd8, 0xf2, 0x2d, 0xf3, 0xd9, 0x2a, 0xd8, 0xf2, 0x35, 0xf3, - 0xd9, 0x32, 0xd8, 0x81, 0xa4, 0x60, 0x60, 0x61, 0xd9, 0x61, 0xd8, 0x6c, 0x68, 0x69, 0xd9, 0x69, - 0xd8, 0x74, 0x70, 0x71, 0xd9, 0x71, 0xd8, 0xb1, 0xa3, 0x84, 0x19, 0x3d, 0x5d, 0xa3, 0x83, 0x1a, - 0x3e, 0x5e, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xb8, 0xb0, 0xaf, 0x8f, 0x94, 0xf2, 0xda, 0x3e, - 0xd8, 0xb4, 0x9a, 0xa8, 0x87, 0x29, 0xda, 0xf8, 0xd8, 0x87, 0x9a, 0x35, 0xda, 0xf8, 0xd8, 0x87, - 0x9a, 0x3d, 0xda, 0xf8, 0xd8, 0xb1, 0xb9, 0xa4, 0x98, 0x85, 0x02, 0x2e, 0x56, 0xa5, 0x81, 0x00, - 0x0c, 0x14, 0xa3, 0x97, 0xb0, 0x8a, 0xf1, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, - 0xd9, 0x68, 0xd8, 0xb1, 0x84, 0x0d, 0xda, 0x0e, 0xd8, 0xa3, 0x29, 0x83, 0xda, 0x2c, 0x0e, 0xd8, - 0xa3, 0x84, 0x49, 0x83, 0xda, 0x2c, 0x4c, 0x0e, 0xd8, 0xb8, 0xb0, 0xa8, 0x8a, 0x9a, 0xf5, 0x20, - 0xaa, 0xda, 0xdf, 0xd8, 0xa8, 0x40, 0xaa, 0xd0, 0xda, 0xde, 0xd8, 0xa8, 0x60, 0xaa, 0xda, 0xd0, - 0xdf, 0xd8, 0xf1, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, - 0xf0, 0x0c, 0x20, 0x14, 0x40, 0xb8, 0xb0, 0xb4, 0xa8, 0x8c, 0x9c, 0xf0, 0x04, 0x28, 0x51, 0x79, - 0x1d, 0x30, 0x14, 0x38, 0xb2, 0x82, 0xab, 0xd0, 0x98, 0x2c, 0x50, 0x50, 0x78, 0x78, 0x9b, 0xf1, - 0x1a, 0xb0, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, - 0x59, 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, - 0x31, 0x8b, 0x30, 0x49, 0x60, 0xa5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, - 0x49, 0x30, 0x19, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, - 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, - 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, - 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, - 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, - 0x88, 0x2d, 0x55, 0x7d, 0x9e, 0xb9, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, - 0xb1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xb8, 0xad, 0x00, 0x2c, 0x54, - 0x7c, 0xf2, 0xb1, 0x8c, 0xb4, 0x99, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0x81, 0x91, 0xac, 0x38, 0xad, - 0x3a, 0xb5, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, - 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9, 0x04, 0xae, 0xd8, 0x79, 0xd9, - 0x04, 0xd8, 0x81, 0xf3, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81, 0xad, 0xd9, 0x01, 0xd8, 0xf2, - 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad, 0xad, 0xad, 0xad, 0xf3, 0x2a, - 0xd8, 0xd8, 0xf1, 0xb0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xf3, 0xac, 0x2e, 0x2e, 0xf1, 0xb1, - 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30, 0x18, 0xa8, 0x98, 0x81, 0x28, - 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xf2, 0xb0, 0x89, 0xac, 0x91, 0x2c, - 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8, 0xd8, 0x79, 0xd9, 0xd8, 0xd8, - 0xf1, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, - 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xb1, 0x83, 0x93, 0x35, 0x3d, 0x80, 0x25, 0xda, 0xd8, 0xd8, 0x85, - 0x69, 0xda, 0xd8, 0xd8, 0xb4, 0x93, 0x81, 0xa3, 0x28, 0x34, 0x3c, 0xf3, 0xab, 0x8b, 0xf8, 0xa3, - 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xfa, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xa3, - 0xa3, 0xa3, 0xa3, 0x95, 0xf1, 0xa3, 0xa3, 0xa3, 0x9d, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, 0xf2, 0xa3, - 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xb2, 0xa3, 0xa3, 0xa3, 0xa3, - 0xa3, 0xa3, 0xb0, 0x87, 0xb5, 0x99, 0xf1, 0xa3, 0xa3, 0xa3, 0x98, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, - 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xb9, 0xa7, 0xf1, 0x26, 0x26, 0x26, - 0xd8, 0xd8, 0xff -}; - -static unsigned short sStartAddress = 0x0300; - - -static tKeyLabel keys[NUM_KEYS]; - -static unsigned short inv_setup_dmpGetAddress( unsigned short key ) -{ - static int isSorted = 0; - if ( !isSorted ) { - int kk; - for (kk=0; kk<NUM_KEYS; ++kk) { - keys[ kk ].addr = 0xffff; - keys[ kk ].key = kk; - } - for (kk=0; kk<NUM_LOCAL_KEYS; ++kk) { - keys[ dmpTConfig[kk].key ].addr = dmpTConfig[kk].addr; - } - isSorted = 1; - } - if ( key >= NUM_KEYS ) - return 0xffff; - return keys[ key ].addr; -} - -/** - * @brief - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_setup_dmp(void) - -{ - inv_error_t result; - - inv_set_get_address(inv_setup_dmpGetAddress); - result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_full_scale(2000.f); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_load_dmp(dmpMemory, DMP_CODE_SIZE, sStartAddress); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_external_sync(MPU_EXT_SYNC_TEMP); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} -/** - *@} - */ diff --git a/60xx/mlsdk/mllite/dmpKey.h b/60xx/mlsdk/mllite/dmpKey.h deleted file mode 100644 index f152644..0000000 --- a/60xx/mlsdk/mllite/dmpKey.h +++ /dev/null @@ -1,432 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef DMPKEY_H__ -#define DMPKEY_H__ - -#define KEY_CFG_25 0 -#define KEY_CFG_24 (KEY_CFG_25+1) -#define KEY_CFG_26 (KEY_CFG_24+1) -#define KEY_CFG_21 (KEY_CFG_26+1) -#define KEY_CFG_20 (KEY_CFG_21+1) -#define KEY_CFG_TAP4 (KEY_CFG_20+1) -#define KEY_CFG_TAP5 (KEY_CFG_TAP4+1) -#define KEY_CFG_TAP6 (KEY_CFG_TAP5+1) -#define KEY_CFG_TAP7 (KEY_CFG_TAP6+1) -#define KEY_CFG_TAP0 (KEY_CFG_TAP7+1) -#define KEY_CFG_TAP1 (KEY_CFG_TAP0+1) -#define KEY_CFG_TAP2 (KEY_CFG_TAP1+1) -#define KEY_CFG_TAP3 (KEY_CFG_TAP2+1) -#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3+1) -#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE+1) -#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_TAP_JERK+1) -#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB+1) -#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_TAP_CLEAR_STICKY +1) -#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT+1) -#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT+1) -#define KEY_FCFG_1 (KEY_CFG_23+1) -#define KEY_FCFG_3 (KEY_FCFG_1+1) -#define KEY_FCFG_2 (KEY_FCFG_3+1) -#define KEY_CFG_3D (KEY_FCFG_2+1) -#define KEY_CFG_3B (KEY_CFG_3D+1) -#define KEY_CFG_3C (KEY_CFG_3B+1) -#define KEY_FCFG_5 (KEY_CFG_3C+1) -#define KEY_FCFG_4 (KEY_FCFG_5+1) -#define KEY_FCFG_7 (KEY_FCFG_4+1) -#define KEY_FCFG_FSCALE (KEY_FCFG_7+1) -#define KEY_FCFG_AZ (KEY_FCFG_FSCALE+1) -#define KEY_FCFG_6 (KEY_FCFG_AZ+1) -#define KEY_FCFG_LSB4 (KEY_FCFG_6+1) -#define KEY_CFG_12 (KEY_FCFG_LSB4+1) -#define KEY_CFG_14 (KEY_CFG_12+1) -#define KEY_CFG_15 (KEY_CFG_14+1) -#define KEY_CFG_16 (KEY_CFG_15+1) -#define KEY_CFG_18 (KEY_CFG_16+1) -#define KEY_CFG_6 (KEY_CFG_18 + 1) -#define KEY_CFG_7 (KEY_CFG_6+1) -#define KEY_CFG_4 (KEY_CFG_7+1) -#define KEY_CFG_5 (KEY_CFG_4+1) -#define KEY_CFG_2 (KEY_CFG_5+1) -#define KEY_CFG_3 (KEY_CFG_2+1) -#define KEY_CFG_1 (KEY_CFG_3+1) -#define KEY_CFG_EXTERNAL (KEY_CFG_1+1) -#define KEY_CFG_8 (KEY_CFG_EXTERNAL+1) -#define KEY_CFG_9 (KEY_CFG_8+1) -#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1) -#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1) -#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1) -#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1) -#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1) -#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1) -#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1) -#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1) -#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1) - -#define KEY_D_0_22 (KEY_FCFG_MAG_MOV + 1) -#define KEY_D_0_24 (KEY_D_0_22+1) -#define KEY_D_0_36 (KEY_D_0_24+1) -#define KEY_D_0_52 (KEY_D_0_36+1) -#define KEY_D_0_96 (KEY_D_0_52+1) -#define KEY_D_0_104 (KEY_D_0_96+1) -#define KEY_D_0_108 (KEY_D_0_104+1) -#define KEY_D_0_163 (KEY_D_0_108+1) -#define KEY_D_0_188 (KEY_D_0_163+1) -#define KEY_D_0_192 (KEY_D_0_188+1) -#define KEY_D_0_224 (KEY_D_0_192+1) -#define KEY_D_0_228 (KEY_D_0_224+1) -#define KEY_D_0_232 (KEY_D_0_228+1) -#define KEY_D_0_236 (KEY_D_0_232+1) - -#define KEY_DMP_PREVPTAT (KEY_D_0_236+1) -#define KEY_D_1_2 (KEY_DMP_PREVPTAT+1) -#define KEY_D_1_4 (KEY_D_1_2 + 1) -#define KEY_D_1_8 (KEY_D_1_4 + 1) -#define KEY_D_1_10 (KEY_D_1_8+1) -#define KEY_D_1_24 (KEY_D_1_10+1) -#define KEY_D_1_28 (KEY_D_1_24+1) -#define KEY_D_1_92 (KEY_D_1_28+1) -#define KEY_D_1_96 (KEY_D_1_92+1) -#define KEY_D_1_98 (KEY_D_1_96+1) -#define KEY_D_1_106 (KEY_D_1_98+1) -#define KEY_D_1_108 (KEY_D_1_106+1) -#define KEY_D_1_112 (KEY_D_1_108+1) -#define KEY_D_1_128 (KEY_D_1_112+1) -#define KEY_D_1_152 (KEY_D_1_128+1) -#define KEY_D_1_168 (KEY_D_1_152+1) -#define KEY_D_1_175 (KEY_D_1_168+1) -#define KEY_D_1_178 (KEY_D_1_175+1) -#define KEY_D_1_179 (KEY_D_1_178+1) -#define KEY_D_1_236 (KEY_D_1_179+1) -#define KEY_D_1_244 (KEY_D_1_236+1) -#define KEY_D_2_12 (KEY_D_1_244+1) -#define KEY_D_2_96 (KEY_D_2_12+1) -#define KEY_D_2_108 (KEY_D_2_96+1) -#define KEY_D_2_244 (KEY_D_2_108+1) -#define KEY_D_2_248 (KEY_D_2_244+1) -#define KEY_D_2_252 (KEY_D_2_248+1) - -// Compass Keys -#define KEY_CPASS_BIAS_X (KEY_D_2_252+1) -#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X+1) -#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y+1) -#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z+1) -#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00+1) -#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01+1) -#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02+1) -#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10+1) -#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11+1) -#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12+1) -#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20+1) -#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21+1) - -// Mantis Keys -#define KEY_CFG_MOTION_BIAS (KEY_CPASS_MTX_22+1) - -#define KEY_DMP_TAPW_MIN (KEY_CFG_MOTION_BIAS+1) -#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN+1) -#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X+1) -#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y+1) -#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z+1) -#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y+1) -#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X+1) -#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z+1) -#define KEY_D_ACT0 (KEY_DMP_ORIENT+1) -#define KEY_D_ACSX (KEY_D_ACT0+1) -#define KEY_D_ACSY (KEY_D_ACSX+1) -#define KEY_D_ACSZ (KEY_D_ACSY+1) - -// Pedometer Standalone only keys -#define KEY_D_PEDSTD_BP_B (KEY_D_ACSZ+1) -#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B+1) -#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A+1) -#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B+1) -#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4+1) -#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3+1) -#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2+1) -#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1+1) -#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH+1) -#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP+1) -#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB+1) -#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME+1) -#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH+1) -#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML+1) -#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH+1) -#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK+1) -#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR+1) -#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR+1) - -// EIS Keys -#define KEY_P_EIS_FIFO_FOOTER (KEY_D_PEDSTD_WALKTIME+1) -#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER+1) -#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT+1) -#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE+1) -#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT+1) -#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC+1) -#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT+1) -#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY+1) -#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER+1) -#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC+1) -#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH+1) -#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH +1) -#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH+1) -#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH+1) -#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH+1) -#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H+1) -#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H+1) -#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H+1) -#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H+1) -#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H+1) -#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H+1) -#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH+1) -#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH+1) -#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH+1) -#define KEY_DMP_INTY_H (KEY_DMP_INTX_H+1) -#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H+1) -#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H+1) -#define KEY_DMP_HPY_H (KEY_DMP_HPX_H+1) -#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H+1) - -// Stream Keys -#define KEY_STREAM_P_GYRO_Z (KEY_DMP_HPZ_H + 1) -#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z+1) -#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y+1) -#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X+1) -#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP+1) -#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y+1) -#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X+1) -#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z+1) -#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y+1) -#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X+1) -#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER+1) - -#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z+1) - - typedef struct { - unsigned short key; - unsigned short addr; - } tKeyLabel; - -#define DINA0A 0x0a -#define DINA22 0x22 -#define DINA42 0x42 -#define DINA5A 0x5a - -#define DINA06 0x06 -#define DINA0E 0x0e -#define DINA16 0x16 -#define DINA1E 0x1e -#define DINA26 0x26 -#define DINA2E 0x2e -#define DINA36 0x36 -#define DINA3E 0x3e -#define DINA46 0x46 -#define DINA4E 0x4e -#define DINA56 0x56 -#define DINA5E 0x5e -#define DINA66 0x66 -#define DINA6E 0x6e -#define DINA76 0x76 -#define DINA7E 0x7e - -#define DINA00 0x00 -#define DINA08 0x08 -#define DINA10 0x10 -#define DINA18 0x18 -#define DINA20 0x20 -#define DINA28 0x28 -#define DINA30 0x30 -#define DINA38 0x38 -#define DINA40 0x40 -#define DINA48 0x48 -#define DINA50 0x50 -#define DINA58 0x58 -#define DINA60 0x60 -#define DINA68 0x68 -#define DINA70 0x70 -#define DINA78 0x78 - -#define DINA04 0x04 -#define DINA0C 0x0c -#define DINA14 0x14 -#define DINA1C 0x1C -#define DINA24 0x24 -#define DINA2C 0x2c -#define DINA34 0x34 -#define DINA3C 0x3c -#define DINA44 0x44 -#define DINA4C 0x4c -#define DINA54 0x54 -#define DINA5C 0x5c -#define DINA64 0x64 -#define DINA6C 0x6c -#define DINA74 0x74 -#define DINA7C 0x7c - -#define DINA01 0x01 -#define DINA09 0x09 -#define DINA11 0x11 -#define DINA19 0x19 -#define DINA21 0x21 -#define DINA29 0x29 -#define DINA31 0x31 -#define DINA39 0x39 -#define DINA41 0x41 -#define DINA49 0x49 -#define DINA51 0x51 -#define DINA59 0x59 -#define DINA61 0x61 -#define DINA69 0x69 -#define DINA71 0x71 -#define DINA79 0x79 - -#define DINA25 0x25 -#define DINA2D 0x2d -#define DINA35 0x35 -#define DINA3D 0x3d -#define DINA4D 0x4d -#define DINA55 0x55 -#define DINA5D 0x5D -#define DINA6D 0x6d -#define DINA75 0x75 -#define DINA7D 0x7d - -#define DINC00 0x00 -#define DINC01 0x01 -#define DINC02 0x02 -#define DINC03 0x03 -#define DINC08 0x08 -#define DINC09 0x09 -#define DINC0A 0x0a -#define DINC0B 0x0b -#define DINC10 0x10 -#define DINC11 0x11 -#define DINC12 0x12 -#define DINC13 0x13 -#define DINC18 0x18 -#define DINC19 0x19 -#define DINC1A 0x1a -#define DINC1B 0x1b - -#define DINC20 0x20 -#define DINC21 0x21 -#define DINC22 0x22 -#define DINC23 0x23 -#define DINC28 0x28 -#define DINC29 0x29 -#define DINC2A 0x2a -#define DINC2B 0x2b -#define DINC30 0x30 -#define DINC31 0x31 -#define DINC32 0x32 -#define DINC33 0x33 -#define DINC38 0x38 -#define DINC39 0x39 -#define DINC3A 0x3a -#define DINC3B 0x3b - -#define DINC40 0x40 -#define DINC41 0x41 -#define DINC42 0x42 -#define DINC43 0x43 -#define DINC48 0x48 -#define DINC49 0x49 -#define DINC4A 0x4a -#define DINC4B 0x4b -#define DINC50 0x50 -#define DINC51 0x51 -#define DINC52 0x52 -#define DINC53 0x53 -#define DINC58 0x58 -#define DINC59 0x59 -#define DINC5A 0x5a -#define DINC5B 0x5b - -#define DINC60 0x60 -#define DINC61 0x61 -#define DINC62 0x62 -#define DINC63 0x63 -#define DINC68 0x68 -#define DINC69 0x69 -#define DINC6A 0x6a -#define DINC6B 0x6b -#define DINC70 0x70 -#define DINC71 0x71 -#define DINC72 0x72 -#define DINC73 0x73 -#define DINC78 0x78 -#define DINC79 0x79 -#define DINC7A 0x7a -#define DINC7B 0x7b - -#if defined CONFIG_MPU_SENSORS_MPU3050 -#define DINA80 0x80 -#define DINA90 0x90 -#define DINAA0 0xa0 -#define DINAC9 0xc9 -#define DINACB 0xcb -#define DINACD 0xcd -#define DINACF 0xcf -#define DINAC8 0xc8 -#define DINACA 0xca -#define DINACC 0xcc -#define DINACE 0xce -#define DINAD8 0xd8 -#define DINADD 0xdd -#define DINAF8 0xf8 -#define DINAFE 0xfe -#define DINAC0 0xc0 -#define DINAC1 0xc1 -#define DINAC2 0xc2 -#define DINAC3 0xc3 -#define DINAC4 0xc4 -#define DINAC5 0xc5 -#elif defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - -#define DINA80 0x80 -#define DINA90 0x90 -#define DINAA0 0xa0 -#define DINAC9 0xc9 -#define DINACB 0xcb -#define DINACD 0xcd -#define DINACF 0xcf -#define DINAC8 0xc8 -#define DINACA 0xca -#define DINACC 0xcc -#define DINACE 0xce -#define DINAD8 0xd8 -#define DINADD 0xdd -#define DINAF8 0xf0 -#define DINAFE 0xfe - -#define DINBF8 0xf8 -#define DINAC0 0xb0 -#define DINAC1 0xb1 -#define DINAC2 0xb4 -#define DINAC3 0xb5 -#define DINAC4 0xb8 -#define DINAC5 0xb9 -#define DINBC0 0xc0 -#define DINBC2 0xc2 -#define DINBC4 0xc4 -#define DINBC6 0xc6 -#else -#error No CONFIG_MPU_SENSORS_xxxx has been defined. -#endif - - -#endif // DMPKEY_H__ diff --git a/60xx/mlsdk/mllite/dmpconfig.txt b/60xx/mlsdk/mllite/dmpconfig.txt deleted file mode 100644 index 4643ed5..0000000 --- a/60xx/mlsdk/mllite/dmpconfig.txt +++ /dev/null @@ -1,3 +0,0 @@ -major version = 1 -minor version = 0 -startAddr = 0 diff --git a/60xx/mlsdk/mllite/dmpmap.h b/60xx/mlsdk/mllite/dmpmap.h deleted file mode 100644 index cb53c7c..0000000 --- a/60xx/mlsdk/mllite/dmpmap.h +++ /dev/null @@ -1,276 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef DMPMAP_H -#define DMPMAP_H - -#ifdef __cplusplus -extern "C" -{ -#endif - -#define DMP_PTAT 0 -#define DMP_XGYR 2 -#define DMP_YGYR 4 -#define DMP_ZGYR 6 -#define DMP_XACC 8 -#define DMP_YACC 10 -#define DMP_ZACC 12 -#define DMP_ADC1 14 -#define DMP_ADC2 16 -#define DMP_ADC3 18 -#define DMP_BIASUNC 20 -#define DMP_FIFORT 22 -#define DMP_INVGSFH 24 -#define DMP_INVGSFL 26 -#define DMP_1H 28 -#define DMP_1L 30 -#define DMP_BLPFSTCH 32 -#define DMP_BLPFSTCL 34 -#define DMP_BLPFSXH 36 -#define DMP_BLPFSXL 38 -#define DMP_BLPFSYH 40 -#define DMP_BLPFSYL 42 -#define DMP_BLPFSZH 44 -#define DMP_BLPFSZL 46 -#define DMP_BLPFMTC 48 -#define DMP_SMC 50 -#define DMP_BLPFMXH 52 -#define DMP_BLPFMXL 54 -#define DMP_BLPFMYH 56 -#define DMP_BLPFMYL 58 -#define DMP_BLPFMZH 60 -#define DMP_BLPFMZL 62 -#define DMP_BLPFC 64 -#define DMP_SMCTH 66 -#define DMP_0H2 68 -#define DMP_0L2 70 -#define DMP_BERR2H 72 -#define DMP_BERR2L 74 -#define DMP_BERR2NH 76 -#define DMP_SMCINC 78 -#define DMP_ANGVBXH 80 -#define DMP_ANGVBXL 82 -#define DMP_ANGVBYH 84 -#define DMP_ANGVBYL 86 -#define DMP_ANGVBZH 88 -#define DMP_ANGVBZL 90 -#define DMP_BERR1H 92 -#define DMP_BERR1L 94 -#define DMP_ATCH 96 -#define DMP_BIASUNCSF 98 -#define DMP_ACT2H 100 -#define DMP_ACT2L 102 -#define DMP_GSFH 104 -#define DMP_GSFL 106 -#define DMP_GH 108 -#define DMP_GL 110 -#define DMP_0_5H 112 -#define DMP_0_5L 114 -#define DMP_0_0H 116 -#define DMP_0_0L 118 -#define DMP_1_0H 120 -#define DMP_1_0L 122 -#define DMP_1_5H 124 -#define DMP_1_5L 126 -#define DMP_TMP1AH 128 -#define DMP_TMP1AL 130 -#define DMP_TMP2AH 132 -#define DMP_TMP2AL 134 -#define DMP_TMP3AH 136 -#define DMP_TMP3AL 138 -#define DMP_TMP4AH 140 -#define DMP_TMP4AL 142 -#define DMP_XACCW 144 -#define DMP_TMP5 146 -#define DMP_XACCB 148 -#define DMP_TMP8 150 -#define DMP_YACCB 152 -#define DMP_TMP9 154 -#define DMP_ZACCB 156 -#define DMP_TMP10 158 -#define DMP_DZH 160 -#define DMP_DZL 162 -#define DMP_XGCH 164 -#define DMP_XGCL 166 -#define DMP_YGCH 168 -#define DMP_YGCL 170 -#define DMP_ZGCH 172 -#define DMP_ZGCL 174 -#define DMP_YACCW 176 -#define DMP_TMP7 178 -#define DMP_AFB1H 180 -#define DMP_AFB1L 182 -#define DMP_AFB2H 184 -#define DMP_AFB2L 186 -#define DMP_MAGFBH 188 -#define DMP_MAGFBL 190 -#define DMP_QT1H 192 -#define DMP_QT1L 194 -#define DMP_QT2H 196 -#define DMP_QT2L 198 -#define DMP_QT3H 200 -#define DMP_QT3L 202 -#define DMP_QT4H 204 -#define DMP_QT4L 206 -#define DMP_CTRL1H 208 -#define DMP_CTRL1L 210 -#define DMP_CTRL2H 212 -#define DMP_CTRL2L 214 -#define DMP_CTRL3H 216 -#define DMP_CTRL3L 218 -#define DMP_CTRL4H 220 -#define DMP_CTRL4L 222 -#define DMP_CTRLS1 224 -#define DMP_CTRLSF1 226 -#define DMP_CTRLS2 228 -#define DMP_CTRLSF2 230 -#define DMP_CTRLS3 232 -#define DMP_CTRLSFNLL 234 -#define DMP_CTRLS4 236 -#define DMP_CTRLSFNL2 238 -#define DMP_CTRLSFNL 240 -#define DMP_TMP30 242 -#define DMP_CTRLSFJT 244 -#define DMP_TMP31 246 -#define DMP_TMP11 248 -#define DMP_CTRLSF2_2 250 -#define DMP_TMP12 252 -#define DMP_CTRLSF1_2 254 -#define DMP_PREVPTAT 256 -#define DMP_ACCZB 258 -#define DMP_ACCXB 264 -#define DMP_ACCYB 266 -#define DMP_1HB 272 -#define DMP_1LB 274 -#define DMP_0H 276 -#define DMP_0L 278 -#define DMP_ASR22H 280 -#define DMP_ASR22L 282 -#define DMP_ASR6H 284 -#define DMP_ASR6L 286 -#define DMP_TMP13 288 -#define DMP_TMP14 290 -#define DMP_FINTXH 292 -#define DMP_FINTXL 294 -#define DMP_FINTYH 296 -#define DMP_FINTYL 298 -#define DMP_FINTZH 300 -#define DMP_FINTZL 302 -#define DMP_TMP1BH 304 -#define DMP_TMP1BL 306 -#define DMP_TMP2BH 308 -#define DMP_TMP2BL 310 -#define DMP_TMP3BH 312 -#define DMP_TMP3BL 314 -#define DMP_TMP4BH 316 -#define DMP_TMP4BL 318 -#define DMP_STXG 320 -#define DMP_ZCTXG 322 -#define DMP_STYG 324 -#define DMP_ZCTYG 326 -#define DMP_STZG 328 -#define DMP_ZCTZG 330 -#define DMP_CTRLSFJT2 332 -#define DMP_CTRLSFJTCNT 334 -#define DMP_PVXG 336 -#define DMP_TMP15 338 -#define DMP_PVYG 340 -#define DMP_TMP16 342 -#define DMP_PVZG 344 -#define DMP_TMP17 346 -#define DMP_MNMFLAGH 352 -#define DMP_MNMFLAGL 354 -#define DMP_MNMTMH 356 -#define DMP_MNMTML 358 -#define DMP_MNMTMTHRH 360 -#define DMP_MNMTMTHRL 362 -#define DMP_MNMTHRH 364 -#define DMP_MNMTHRL 366 -#define DMP_ACCQD4H 368 -#define DMP_ACCQD4L 370 -#define DMP_ACCQD5H 372 -#define DMP_ACCQD5L 374 -#define DMP_ACCQD6H 376 -#define DMP_ACCQD6L 378 -#define DMP_ACCQD7H 380 -#define DMP_ACCQD7L 382 -#define DMP_ACCQD0H 384 -#define DMP_ACCQD0L 386 -#define DMP_ACCQD1H 388 -#define DMP_ACCQD1L 390 -#define DMP_ACCQD2H 392 -#define DMP_ACCQD2L 394 -#define DMP_ACCQD3H 396 -#define DMP_ACCQD3L 398 -#define DMP_XN2H 400 -#define DMP_XN2L 402 -#define DMP_XN1H 404 -#define DMP_XN1L 406 -#define DMP_YN2H 408 -#define DMP_YN2L 410 -#define DMP_YN1H 412 -#define DMP_YN1L 414 -#define DMP_YH 416 -#define DMP_YL 418 -#define DMP_B0H 420 -#define DMP_B0L 422 -#define DMP_A1H 424 -#define DMP_A1L 426 -#define DMP_A2H 428 -#define DMP_A2L 430 -#define DMP_SEM1 432 -#define DMP_FIFOCNT 434 -#define DMP_SH_TH_X 436 -#define DMP_PACKET 438 -#define DMP_SH_TH_Y 440 -#define DMP_FOOTER 442 -#define DMP_SH_TH_Z 444 -#define DMP_TEMP29 448 -#define DMP_TEMP30 450 -#define DMP_XACCB_PRE 452 -#define DMP_XACCB_PREL 454 -#define DMP_YACCB_PRE 456 -#define DMP_YACCB_PREL 458 -#define DMP_ZACCB_PRE 460 -#define DMP_ZACCB_PREL 462 -#define DMP_TMP22 464 -#define DMP_TAP_TIMER 466 -#define DMP_TAP_THX 468 -#define DMP_TAP_THY 472 -#define DMP_TAP_THZ 476 -#define DMP_TAPW_MIN 478 -#define DMP_TMP25 480 -#define DMP_TMP26 482 -#define DMP_TMP27 484 -#define DMP_TMP28 486 -#define DMP_ORIENT 488 -#define DMP_THRSH 490 -#define DMP_ENDIANH 492 -#define DMP_ENDIANL 494 -#define DMP_BLPFNMTCH 496 -#define DMP_BLPFNMTCL 498 -#define DMP_BLPFNMXH 500 -#define DMP_BLPFNMXL 502 -#define DMP_BLPFNMYH 504 -#define DMP_BLPFNMYL 506 -#define DMP_BLPFNMZH 508 -#define DMP_BLPFNMZL 510 -#ifdef __cplusplus -} -#endif -#endif // DMPMAP_H diff --git a/60xx/mlsdk/mllite/invensense.h b/60xx/mlsdk/mllite/invensense.h deleted file mode 100644 index 586dd25..0000000 --- a/60xx/mlsdk/mllite/invensense.h +++ /dev/null @@ -1,24 +0,0 @@ -/** Main header file for Invensense's basic library. - */ -#include "accel.h" -#include "compass.h" -#include "dmpDefault.h" -#include "dmpKey.h" -#include "dmpmap.h" -#include "ml.h" -#include "mlBiasNoMotion.h" -#include "mlFIFO.h" -#include "mlFIFOHW.h" -#include "mlMathFunc.h" -#include "mlSetGyroBias.h" -#include "ml_legacy.h" -#include "ml_mputest.h" -#include "ml_stored_data.h" -#include "mlcontrol.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mldmp.h" -#include "mlinclude.h" -#include "mlstates.h" -#include "mlsupervisor.h" -#include "pressure.h" diff --git a/60xx/mlsdk/mllite/ml.c b/60xx/mlsdk/mllite/ml.c deleted file mode 100644 index 3328edd..0000000 --- a/60xx/mlsdk/mllite/ml.c +++ /dev/null @@ -1,1885 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: ml.c 5642 2011-06-14 02:26:20Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup ML - * @brief Motion Library APIs. - * The Motion Library processes gyroscopes, accelerometers, and - * compasses to provide a physical model of the movement for the - * sensors. - * The results of this processing may be used to control objects - * within a user interface environment, detect gestures, track 3D - * movement for gaming applications, and analyze the blur created - * due to hand movement while taking a picture. - * - * @{ - * @file ml.c - * @brief The Motion Library APIs. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> - -#include "ml.h" -#include "mldl.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "compass.h" -#include "dmpKey.h" -#include "dmpDefault.h" -#include "mlstates.h" -#include "mlFIFO.h" -#include "mlFIFOHW.h" -#include "mlMathFunc.h" -#include "mlsupervisor.h" -#include "mlmath.h" -#include "mlcontrol.h" -#include "mldl_cfg.h" -#include "mpu.h" -#include "accel.h" -#include "mlos.h" -#include "mlsl.h" -#include "mlos.h" -#include "mlBiasNoMotion.h" -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-ml" - -#define ML_MOT_TYPE_NONE 0 -#define ML_MOT_TYPE_NO_MOTION 1 -#define ML_MOT_TYPE_MOTION_DETECTED 2 - -#define ML_MOT_STATE_MOVING 0 -#define ML_MOT_STATE_NO_MOTION 1 -#define ML_MOT_STATE_BIAS_IN_PROG 2 - -#define _mlDebug(x) //{x} - -/* Global Variables */ -const unsigned char mlVer[] = { INV_VERSION }; - -struct inv_params_obj inv_params_obj = { - INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode - INV_ORIENTATION_MASK_DEFAULT, // orientation_mask - INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func - INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func - INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func - INV_STATE_SERIAL_CLOSED // starting state -}; - -struct inv_obj_t inv_obj; -void *g_mlsl_handle; - -typedef struct { - // These describe callbacks happening everythime a DMP interrupt is processed - int_fast8_t numInterruptProcesses; - // Array of function pointers, function being void function taking void - inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES]; - -} tMLXCallbackInterrupt; // MLX_callback_t - -tMLXCallbackInterrupt mlxCallbackInterrupt; - -/* --------------- */ -/* - Functions. - */ -/* --------------- */ - -inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient); -inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient); -unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); - -/** - * @brief Open serial connection with the MPU device. - * This is the entry point of the MPL and must be - * called prior to any other function call. - * - * @param port System handle for 'port' MPU device is found on. - * The significance of this parameter varies by - * platform. It is passed as 'port' to MLSLSerialOpen. - * - * @return INV_SUCCESS or error code. - */ -inv_error_t inv_serial_start(char const *port) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - if (inv_get_state() >= INV_STATE_SERIAL_OPENED) - return INV_SUCCESS; - - result = inv_state_transition(INV_STATE_SERIAL_OPENED); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_serial_open(port, &g_mlsl_handle); - if (INV_SUCCESS != result) { - (void)inv_state_transition(INV_STATE_SERIAL_CLOSED); - } - - return result; -} - -/** - * @brief Close the serial communication. - * This function needs to be called explicitly to shut down - * the communication with the device. Calling inv_dmp_close() - * won't affect the established serial communication. - * @return INV_SUCCESS; non-zero error code otherwise. - */ -inv_error_t inv_serial_stop(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() == INV_STATE_SERIAL_CLOSED) - return INV_SUCCESS; - - result = inv_state_transition(INV_STATE_SERIAL_CLOSED); - if (INV_SUCCESS != result) { - MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result); - } - result = inv_serial_close(g_mlsl_handle); - if (INV_SUCCESS != result) { - MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result); - } - return result; -} - -/** - * @brief Get the serial file handle to the device. - * @return The serial file handle. - */ -void *inv_get_serial_handle(void) -{ - INVENSENSE_FUNC_START; - return g_mlsl_handle; -} - -/** - * @brief apply the choosen orientation and full scale range - * for gyroscopes, accelerometer, and compass. - * @return INV_SUCCESS if successful, a non-zero code otherwise. - */ -inv_error_t inv_apply_calibration(void) -{ - INVENSENSE_FUNC_START; - signed char gyroCal[9] = { 0 }; - signed char accelCal[9] = { 0 }; - signed char magCal[9] = { 0 }; - float gyroScale = 2000.f; - float accelScale = 0.f; - float magScale = 0.f; - - inv_error_t result; - int ii; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - for (ii = 0; ii < 9; ii++) { - gyroCal[ii] = mldl_cfg->pdata->orientation[ii]; - if (NULL != mldl_cfg->accel){ - accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii]; - } - if (NULL != mldl_cfg->compass){ - magCal[ii] = mldl_cfg->pdata->compass.orientation[ii]; - } - } - - switch (mldl_cfg->full_scale) { - case MPU_FS_250DPS: - gyroScale = 250.f; - break; - case MPU_FS_500DPS: - gyroScale = 500.f; - break; - case MPU_FS_1000DPS: - gyroScale = 1000.f; - break; - case MPU_FS_2000DPS: - gyroScale = 2000.f; - break; - default: - MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n", - mldl_cfg->full_scale); - return INV_ERROR_INVALID_PARAMETER; - break; - } - - if (NULL != mldl_cfg->accel){ - RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale); - inv_obj.accel_sens = (long)(accelScale * 65536L); - /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */ -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim; -#else - inv_obj.accel_sens /= 2; -#endif - } - if (NULL != mldl_cfg->compass){ - RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale); - inv_obj.compass_sens = (long)(magScale * 32768); - } - - if (inv_get_state() == INV_STATE_DMP_OPENED) { - result = inv_set_gyro_calibration(gyroScale, gyroCal); - if (INV_SUCCESS != result) { - MPL_LOGE("Unable to set Gyro Calibration\n"); - return result; - } - if (NULL != mldl_cfg->accel){ - result = inv_set_accel_calibration(accelScale, accelCal); - if (INV_SUCCESS != result) { - MPL_LOGE("Unable to set Accel Calibration\n"); - return result; - } - } - if (NULL != mldl_cfg->compass){ - result = inv_set_compass_calibration(magScale, magCal); - if (INV_SUCCESS != result) { - MPL_LOGE("Unable to set Mag Calibration\n"); - return result; - } - } - } - return INV_SUCCESS; -} - -/** - * @brief Setup the DMP to handle the accelerometer endianess. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_apply_endian_accel(void) -{ - INVENSENSE_FUNC_START; - unsigned char regs[4] = { 0 }; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - int endian = mldl_cfg->accel->endian; - - if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) { - endian = EXT_SLAVE_BIG_ENDIAN; - } - switch (endian) { - case EXT_SLAVE_FS8_BIG_ENDIAN: - case EXT_SLAVE_FS16_BIG_ENDIAN: - case EXT_SLAVE_LITTLE_ENDIAN: - regs[0] = 0; - regs[1] = 64; - regs[2] = 0; - regs[3] = 0; - break; - case EXT_SLAVE_BIG_ENDIAN: - default: - regs[0] = 0; - regs[1] = 0; - regs[2] = 64; - regs[3] = 0; - } - - return inv_set_mpu_memory(KEY_D_1_236, 4, regs); -} - -/** - * @internal - * @brief Initialize MLX data. This should be called to setup the mlx - * output buffers before any motion processing is done. - */ -void inv_init_ml(void) -{ - INVENSENSE_FUNC_START; - int ii; - long tmp[COMPASS_NUM_AXES]; - // Set all values to zero by default - memset(&inv_obj, 0, sizeof(struct inv_obj_t)); - memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt)); - - inv_obj.compass_correction[0] = 1073741824L; - inv_obj.compass_correction_relative[0] = 1073741824L; - inv_obj.compass_disturb_correction[0] = 1073741824L; - inv_obj.compass_correction_offset[0] = 1073741824L; - inv_obj.relative_quat[0] = 1073741824L; - - //Not used with the ST accelerometer - inv_obj.no_motion_threshold = 20; // noMotionThreshold - //Not used with the ST accelerometer - inv_obj.motion_duration = 1536; // motionDuration - - inv_obj.motion_state = INV_MOTION; // Motion state - - inv_obj.bias_update_time = 8000; - inv_obj.bias_calc_time = 2000; - - inv_obj.internal_motion_state = ML_MOT_STATE_MOVING; - - inv_obj.start_time = inv_get_tick_count(); - - inv_obj.compass_cal[0] = 322122560L; - inv_obj.compass_cal[4] = 322122560L; - inv_obj.compass_cal[8] = 322122560L; - inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed. - - for (ii = 0; ii < COMPASS_NUM_AXES; ii++) { - inv_obj.compass_scale[ii] = 65536L; - inv_obj.compass_test_scale[ii] = 65536L; - inv_obj.compass_bias_error[ii] = P_INIT; - inv_obj.init_compass_bias[ii] = 0; - inv_obj.compass_asa[ii] = (1L << 30); - } - if (inv_compass_read_scale(tmp) == INV_SUCCESS) { - for (ii = 0; ii < COMPASS_NUM_AXES; ii++) - inv_obj.compass_asa[ii] = tmp[ii]; - } - inv_obj.got_no_motion_bias = 0; - inv_obj.got_compass_bias = 0; - inv_obj.compass_state = SF_UNCALIBRATED; - inv_obj.acc_state = SF_STARTUP_SETTLE; - - inv_obj.got_init_compass_bias = 0; - inv_obj.resetting_compass = 0; - - inv_obj.external_slave_callback = NULL; - inv_obj.compass_accuracy = 0; - - inv_obj.factory_temp_comp = 0; - inv_obj.got_coarse_heading = 0; - - inv_obj.compass_bias_ptr[0] = P_INIT; - inv_obj.compass_bias_ptr[4] = P_INIT; - inv_obj.compass_bias_ptr[8] = P_INIT; - - inv_obj.gyro_bias_err = 1310720; - - inv_obj.accel_lpf_gain = 1073744L; - inv_obj.no_motion_accel_threshold = 7000000L; -} - -/** - * @internal - * @brief This registers a function to be called for each time the DMP - * generates an an interrupt. - * It will be called after inv_register_fifo_rate_process() which gets called - * every time the FIFO data is processed. - * The FIFO does not have to be on for this callback. - * @param func Function to be called when a DMP interrupt occurs. - * @return INV_SUCCESS or non-zero error code. - */ -inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func) -{ - INVENSENSE_FUNC_START; - // Make sure we have not filled up our number of allowable callbacks - if (mlxCallbackInterrupt.numInterruptProcesses <= - MAX_INTERRUPT_PROCESSES - 1) { - int kk; - // Make sure we haven't registered this function already - for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { - if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { - return INV_ERROR_INVALID_PARAMETER; - } - } - // Add new callback - mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt. - numInterruptProcesses] = func; - mlxCallbackInterrupt.numInterruptProcesses++; - return INV_SUCCESS; - } - return INV_ERROR_MEMORY_EXAUSTED; -} - -/** - * @internal - * @brief This unregisters a function to be called for each DMP interrupt. - * @return INV_SUCCESS or non-zero error code. - */ -inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func) -{ - INVENSENSE_FUNC_START; - int kk, jj; - // Make sure we haven't registered this function already - for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { - if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { - // FIXME, we may need a thread block here - for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses; - ++jj) { - mlxCallbackInterrupt.processInterruptCb[jj - 1] = - mlxCallbackInterrupt.processInterruptCb[jj]; - } - mlxCallbackInterrupt.numInterruptProcesses--; - return INV_SUCCESS; - } - } - return INV_ERROR_INVALID_PARAMETER; -} - -/** - * @internal - * @brief Run the recorded interrupt process callbacks in the event - * of an interrupt. - */ -void inv_run_dmp_interupt_cb(void) -{ - int kk; - for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { - if (mlxCallbackInterrupt.processInterruptCb[kk]) - mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj); - } -} - -/** @internal -* Resets the Motion/No Motion state which should be called at startup and resume. -*/ -inv_error_t inv_reset_motion(void) -{ - unsigned char regs[8]; - inv_error_t result; - - inv_obj.motion_state = INV_MOTION; - inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; - inv_obj.no_motion_accel_time = inv_get_tick_count(); -#if defined CONFIG_MPU_SENSORS_MPU3050 - regs[0] = DINAD8 + 2; - regs[1] = DINA0C; - regs[2] = DINAD8 + 1; - result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#else -#endif - regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); - regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); - result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - memset(regs, 0, 8); - result = inv_set_mpu_memory(KEY_D_1_96, 8, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = - inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - inv_set_motion_state(INV_MOTION); - return result; -} - -/** - * @internal - * @brief inv_start_bias_calc starts the bias calculation on the MPU. - */ -void inv_start_bias_calc(void) -{ - INVENSENSE_FUNC_START; - - inv_obj.suspend = 1; -} - -/** - * @internal - * @brief inv_stop_bias_calc stops the bias calculation on the MPU. - */ -void inv_stop_bias_calc(void) -{ - INVENSENSE_FUNC_START; - - inv_obj.suspend = 0; -} - -/** - * @brief inv_update_data fetches data from the fifo and updates the - * motion algorithms. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() must have been called. - * - * @note Motion algorithm data is constant between calls to inv_update_data - * - * @return - * - INV_SUCCESS - * - Non-zero error code - */ -inv_error_t inv_update_data(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - int_fast8_t got, ftry; - uint_fast8_t mpu_interrupt; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - // Set the maximum number of FIFO packets we want to process - if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { - ftry = 100; // Large enough to process all packets - } else { - ftry = 1; - } - - // Go and process at most ftry number of packets, probably less than ftry - result = inv_read_and_process_fifo(ftry, &got); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - // Process all interrupts - mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1); - if (mpu_interrupt) { - inv_clear_interrupt_trigger(INTSRC_AUX1); - } - // Check if interrupt was from MPU - mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU); - if (mpu_interrupt) { - inv_clear_interrupt_trigger(INTSRC_MPU); - } - // Take care of the callbacks that want to be notified when there was an MPU interrupt - if (mpu_interrupt) { - inv_run_dmp_interupt_cb(); - } - - result = inv_get_fifo_status(); - return result; -} - -/** - * @brief inv_check_flag returns the value of a flag. - * inv_check_flag can be used to check a number of flags, - * allowing users to poll flags rather than register callback - * functions. If a flag is set to True when inv_check_flag is called, - * the flag is automatically reset. - * The flags are: - * - INV_RAW_DATA_READY - * Indicates that new raw data is available. - * - INV_PROCESSED_DATA_READY - * Indicates that new processed data is available. - * - INV_GOT_GESTURE - * Indicates that a gesture has been detected by the gesture engine. - * - INV_MOTION_STATE_CHANGE - * Indicates that a change has been made from motion to no motion, - * or vice versa. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() must have been called. - * - * @param flag The flag to check. - * - * @return TRUE or FALSE state of the flag - */ -int inv_check_flag(int flag) -{ - INVENSENSE_FUNC_START; - int flagReturn = inv_obj.flags[flag]; - - inv_obj.flags[flag] = 0; - return flagReturn; -} - -/** - * @brief Enable generation of the DMP interrupt when Motion or no-motion - * is detected - * @param on - * Boolean to turn the interrupt on or off. - * @return INV_SUCCESS or non-zero error code. - */ -inv_error_t inv_set_motion_interrupt(unsigned char on) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - unsigned char regs[2] = { 0 }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (on) { - result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - inv_obj.interrupt_sources |= INV_INT_MOTION; - } else { - inv_obj.interrupt_sources &= ~INV_INT_MOTION; - if (!inv_obj.interrupt_sources) { - result = inv_get_dl_cfg_int(0); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - - if (on) { - regs[0] = DINAFE; - } else { - regs[0] = DINAD8; - } - result = inv_set_mpu_memory(KEY_CFG_7, 1, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * Enable generation of the DMP interrupt when a FIFO packet is ready - * - * @param on Boolean to turn the interrupt on or off - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_set_fifo_interrupt(unsigned char on) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - unsigned char regs[2] = { 0 }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (on) { - result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - inv_obj.interrupt_sources |= INV_INT_FIFO; - } else { - inv_obj.interrupt_sources &= ~INV_INT_FIFO; - if (!inv_obj.interrupt_sources) { - result = inv_get_dl_cfg_int(0); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - - if (on) { - regs[0] = DINAFE; - } else { - regs[0] = DINAD8; - } - result = inv_set_mpu_memory(KEY_CFG_6, 1, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief Get the current set of DMP interrupt sources. - * These interrupts are generated by the DMP and can be - * routed to the MPU interrupt line via internal - * settings. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @return Currently enabled interrupt sources. The possible interrupts are: - * - INV_INT_FIFO, - * - INV_INT_MOTION, - * - INV_INT_TAP - */ -int inv_get_interrupts(void) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return 0; // error - - return inv_obj.interrupt_sources; -} - -/** - * @brief Sets up the Accelerometer calibration and scale factor. - * - * Please refer to the provided "9-Axis Sensor Fusion Application - * Note" document provided. Section 5, "Sensor Mounting Orientation" - * offers a good coverage on the mounting matrices and explains how - * to use them. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * @pre inv_dmp_start() must <b>NOT</b> have been called. - * - * @see inv_set_gyro_calibration(). - * @see inv_set_compass_calibration(). - * - * @param[in] range - * The range of the accelerometers in g's. An accelerometer - * that has a range of +2g's to -2g's should pass in 2. - * @param[in] orientation - * A 9 element matrix that represents how the accelerometers - * are oriented with respect to the device they are mounted - * in and the reference axis system. - * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. - * This example corresponds to a 3 x 3 identity matrix. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_accel_calibration(float range, signed char *orientation) -{ - INVENSENSE_FUNC_START; - float cal[9]; - float scale = range / 32768.f; - int kk; - unsigned long sf; - inv_error_t result; - unsigned char regs[4] = { 0, 0, 0, 0 }; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - /* Apply zero g-offset values */ - if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) { - regs[0] = 0x80; - regs[1] = 0x0; - regs[2] = 0x80; - regs[3] = 0x0; - } - - if (inv_dmpkey_supported(KEY_D_1_152)) { - result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - if (scale == 0) { - inv_obj.accel_sens = 0; - } - - if (mldl_cfg->accel->id) { -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C }; -#else - unsigned char tmp[3] = { DINA4C, DINACD, DINA6C }; -#endif - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - unsigned char regs[3]; - unsigned short orient; - - for (kk = 0; kk < 9; ++kk) { - cal[kk] = scale * orientation[kk]; - inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); - } - - orient = inv_orientation_matrix_to_scalar(orientation); - regs[0] = tmp[orient & 3]; - regs[1] = tmp[(orient >> 3) & 3]; - regs[2] = tmp[(orient >> 6) & 3]; - result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - regs[0] = DINA26; - regs[1] = DINA46; -#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2 - regs[2] = DINA66; -#else - if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision) - == MPU_PRODUCT_KEY_B1_E1_5) - regs[2] = DINA76; - else - regs[2] = DINA66; -#endif - if (orient & 4) - regs[0] |= 1; - if (orient & 0x20) - regs[1] |= 1; - if (orient & 0x100) - regs[2] |= 1; - - result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) { - result = inv_freescale_sensor_fusion_16bit(orient); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) { - result = inv_freescale_sensor_fusion_8bit(orient); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - - if (inv_obj.accel_sens != 0) { - sf = (1073741824L / inv_obj.accel_sens); - } else { - sf = 0; - } - regs[0] = (unsigned char)((sf >> 8) & 0xff); - regs[1] = (unsigned char)(sf & 0xff); - result = inv_set_mpu_memory(KEY_D_0_108, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/** - * @brief Sets up the Gyro calibration and scale factor. - * - * Please refer to the provided "9-Axis Sensor Fusion Application - * Note" document provided. Section 5, "Sensor Mounting Orientation" - * offers a good coverage on the mounting matrices and explains - * how to use them. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * @pre inv_dmp_start() must have <b>NOT</b> been called. - * - * @see inv_set_accel_calibration(). - * @see inv_set_compass_calibration(). - * - * @param[in] range - * The range of the gyros in degrees per second. A gyro - * that has a range of +2000 dps to -2000 dps should pass in - * 2000. - * @param[in] orientation - * A 9 element matrix that represents how the gyro are oriented - * with respect to the device they are mounted in. A typical - * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This - * example corresponds to a 3 x 3 identity matrix. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_gyro_calibration(float range, signed char *orientation) -{ - INVENSENSE_FUNC_START; - - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - int kk; - float scale; - inv_error_t result; - - unsigned char regs[12] = { 0 }; - unsigned char maxVal = 0; - unsigned char tmpPtr = 0; - unsigned char tmpSign = 0; - unsigned char i = 0; - int sf = 0; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (mldl_cfg->gyro_sens_trim != 0) { - /* adjust the declared range to consider the gyro_sens_trim - of this part when different from the default (first dividend) */ - range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim; - } - - scale = range / 32768.f; // inverse of sensitivity for the given full scale range - inv_obj.gyro_sens = (long)(range * 32768L); - - for (kk = 0; kk < 9; ++kk) { - inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30)); - inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30)); - } - - { -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - unsigned char tmpD = DINA4C; - unsigned char tmpE = DINACD; - unsigned char tmpF = DINA6C; -#else - unsigned char tmpD = DINAC9; - unsigned char tmpE = DINA2C; - unsigned char tmpF = DINACB; -#endif - regs[3] = DINA36; - regs[4] = DINA56; - regs[5] = DINA76; - - for (i = 0; i < 3; i++) { - maxVal = 0; - tmpSign = 0; - if (inv_obj.gyro_orient[0 + 3 * i] < 0) - tmpSign = 1; - if (ABS(inv_obj.gyro_orient[1 + 3 * i]) > - ABS(inv_obj.gyro_orient[0 + 3 * i])) { - maxVal = 1; - if (inv_obj.gyro_orient[1 + 3 * i] < 0) - tmpSign = 1; - } - if (ABS(inv_obj.gyro_orient[2 + 3 * i]) > - ABS(inv_obj.gyro_orient[1 + 3 * i])) { - tmpSign = 0; - maxVal = 2; - if (inv_obj.gyro_orient[2 + 3 * i] < 0) - tmpSign = 1; - } - if (maxVal == 0) { - regs[tmpPtr++] = tmpD; - if (tmpSign) - regs[tmpPtr + 2] |= 0x01; - } else if (maxVal == 1) { - regs[tmpPtr++] = tmpE; - if (tmpSign) - regs[tmpPtr + 2] |= 0x01; - } else { - regs[tmpPtr++] = tmpF; - if (tmpSign) - regs[tmpPtr + 2] |= 0x01; - } - } - result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384 - inv_obj.gyro_sf = - (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL); - result = - inv_set_mpu_memory(KEY_D_0_104, 4, - inv_int32_to_big8(inv_obj.gyro_sf, regs)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (inv_obj.gyro_sens != 0) { - sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens); - } else { - sf = 0; - } - - result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - return INV_SUCCESS; -} - -/** - * @brief Sets up the Compass calibration and scale factor. - * - * Please refer to the provided "9-Axis Sensor Fusion Application - * Note" document provided. Section 5, "Sensor Mounting Orientation" - * offers a good coverage on the mounting matrices and explains - * how to use them. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * @pre inv_dmp_start() must have <b>NOT</b> been called. - * - * @see inv_set_gyro_calibration(). - * @see inv_set_accel_calibration(). - * - * @param[in] range - * The range of the compass. - * @param[in] orientation - * A 9 element matrix that represents how the compass is - * oriented with respect to the device they are mounted in. - * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. - * This example corresponds to a 3 x 3 identity matrix. - * The matrix describes how to go from the chip mounting to - * the body of the device. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_compass_calibration(float range, signed char *orientation) -{ - INVENSENSE_FUNC_START; - float cal[9]; - float scale = range / 32768.f; - int kk; - unsigned short compassId = 0; - - compassId = inv_get_compass_id(); - - if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883) - || (compassId == COMPASS_ID_LSM303M)) { - scale /= 32.0f; - } - - for (kk = 0; kk < 9; ++kk) { - cal[kk] = scale * orientation[kk]; - inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); - } - - inv_obj.compass_sens = (long)(scale * 1073741824L); - - if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) { - unsigned char reg0[4] = { 0, 0, 0, 0 }; - unsigned char regp[4] = { 64, 0, 0, 0 }; - unsigned char regn[4] = { 64 + 128, 0, 0, 0 }; - unsigned char *reg; - int_fast8_t kk; - unsigned short keyList[9] = - { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02, - KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12, - KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22 - }; - - for (kk = 0; kk < 9; ++kk) { - - if (orientation[kk] == 1) - reg = regp; - else if (orientation[kk] == -1) - reg = regn; - else - reg = reg0; - inv_set_mpu_memory(keyList[kk], 4, reg); - } - } - - return INV_SUCCESS; -} - -/** -* @internal -* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup. -*/ -inv_error_t inv_set_dead_zone(void) -{ - unsigned char reg; - inv_error_t result; - extern struct control_params cntrl_params; - - if (cntrl_params.functions & INV_DEAD_ZONE) { - reg = 0x08; - } else { -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - reg = 0; -#else - if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { - reg = 0x2; - } else { - reg = 0; - } -#endif - } - - result = inv_set_mpu_memory(KEY_D_0_163, 1, ®); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief inv_set_bias_update is used to register which algorithms will be - * used to automatically reset the gyroscope bias. - * The engine INV_BIAS_UPDATE must be enabled for these algorithms to - * run. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() - * must <b>NOT</b> have been called. - * - * @param function A function or bitwise OR of functions that determine - * how the gyroscope bias will be automatically updated. - * Functions include: - * - INV_NONE or 0, - * - INV_BIAS_FROM_NO_MOTION, - * - INV_BIAS_FROM_GRAVITY, - * - INV_BIAS_FROM_TEMPERATURE, - \ifnot UMPL - * - INV_BIAS_FROM_LPF, - * - INV_MAG_BIAS_FROM_MOTION, - * - INV_MAG_BIAS_FROM_GYRO, - * - INV_ALL. - * \endif - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_bias_update(unsigned short function) -{ - INVENSENSE_FUNC_START; - unsigned char regs[4]; - long tmp[3] = { 0, 0, 0 }; - inv_error_t result = INV_SUCCESS; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - /* do not allow progressive no motion bias tracker to run - - it's not fully debugged */ - function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround - MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n"); - - // You must use EnableFastNoMotion() to get this feature - function &= ~INV_BIAS_FROM_FAST_NO_MOTION; - - // You must use DisableFastNoMotion() to turn this feature off - if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) - function |= INV_BIAS_FROM_FAST_NO_MOTION; - - /*--- remove magnetic components from bias tracking - if there is no compass ---*/ - if (!inv_compass_present()) { - function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION); - } else { - function &= ~(INV_BIAS_FROM_LPF); - } - - // Enable function sets bias from no motion - inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION); - - if (function & INV_BIAS_FROM_NO_MOTION) { - inv_enable_bias_no_motion(); - } else { - inv_disable_bias_no_motion(); - } - - if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { - regs[0] = DINA80 + 2; - regs[1] = DINA2D; - regs[2] = DINA55; - regs[3] = DINA7D; - } else { - regs[0] = DINA80 + 7; - regs[1] = DINA2D; - regs[2] = DINA35; - regs[3] = DINA3D; - } - result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_dead_zone(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) && - !inv_compass_present()) { - result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else { - result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - inv_obj.factory_temp_comp = 0; // FIXME, workaround - if ((mldl_cfg->offset_tc[0] != 0) || - (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) { - inv_obj.factory_temp_comp = 1; - } - - if (inv_obj.factory_temp_comp == 0) { - if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) { - result = inv_set_gyro_temp_slope(inv_obj.temp_slope); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else { - result = inv_set_gyro_temp_slope(tmp); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } else { - inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE; - MPL_LOGV("factory temperature compensation coefficients available - " - "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n"); - } - - /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on - compass and accel data, is to have accelerometer data delivered in the - FIFO ----*/ - if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) - && inv_compass_present()) - || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) - || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) { - inv_send_accel(INV_ALL, INV_32_BIT); - inv_send_gyro(INV_ALL, INV_32_BIT); - } - - return result; -} - -/** - * @brief inv_get_motion_state is used to determine if the device is in - * a 'motion' or 'no motion' state. - * inv_get_motion_state returns INV_MOTION of the device is moving, - * or INV_NO_MOTION if the device is not moving. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() - * must have been called. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -int inv_get_motion_state(void) -{ - INVENSENSE_FUNC_START; - return inv_obj.motion_state; -} - -/** - * @brief inv_set_no_motion_thresh is used to set the threshold for - * detecting INV_NO_MOTION - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param thresh A threshold scaled in degrees per second. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_no_motion_thresh(float thresh) -{ - inv_error_t result = INV_SUCCESS; - unsigned char regs[4] = { 0 }; - long tmp; - INVENSENSE_FUNC_START; - - tmp = (long)(thresh * thresh * 2.045f); - if (tmp < 0) { - return INV_ERROR; - } else if (tmp > 8180000L) { - return INV_ERROR; - } - inv_obj.no_motion_threshold = tmp; - - regs[0] = (unsigned char)(tmp >> 24); - regs[1] = (unsigned char)((tmp >> 16) & 0xff); - regs[2] = (unsigned char)((tmp >> 8) & 0xff); - regs[3] = (unsigned char)(tmp & 0xff); - - result = inv_set_mpu_memory(KEY_D_1_108, 4, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_reset_motion(); - return result; -} - -/** - * @brief inv_set_no_motion_threshAccel is used to set the threshold for - * detecting INV_NO_MOTION with accelerometers when Gyros have - * been turned off - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param thresh A threshold in g's scaled by 2^32 - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_no_motion_threshAccel(long thresh) -{ - INVENSENSE_FUNC_START; - - inv_obj.no_motion_accel_threshold = thresh; - - return INV_SUCCESS; -} - -/** - * @brief inv_set_no_motion_time is used to set the time required for - * detecting INV_NO_MOTION - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param time A time in seconds. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_no_motion_time(float time) -{ - inv_error_t result = INV_SUCCESS; - unsigned char regs[2] = { 0 }; - long tmp; - - INVENSENSE_FUNC_START; - - tmp = (long)(time * 200); - if (tmp < 0) { - return INV_ERROR; - } else if (tmp > 65535L) { - return INV_ERROR; - } - inv_obj.motion_duration = (unsigned short)tmp; - - regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); - regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); - result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_reset_motion(); - return result; -} - -/** - * @brief inv_get_version is used to get the ML version. - * - * @pre inv_get_version can be called at any time. - * - * @param version inv_get_version writes the ML version - * string pointer to version. - * - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_get_version(unsigned char **version) -{ - INVENSENSE_FUNC_START; - - *version = (unsigned char *)mlVer; //fixme we are wiping const - - return INV_SUCCESS; -} - -/** - * @brief Check for the presence of the gyro sensor. - * - * This is not a physical check but a logical check and the value can change - * dynamically based on calls to inv_set_mpu_sensors(). - * - * @return TRUE if the gyro is enabled FALSE otherwise. - */ -int inv_get_gyro_present(void) -{ - return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO | - INV_Z_GYRO); -} - -static unsigned short inv_row_2_scale(const signed char *row) -{ - unsigned short b; - - if (row[0] > 0) - b = 0; - else if (row[0] < 0) - b = 4; - else if (row[1] > 0) - b = 1; - else if (row[1] < 0) - b = 5; - else if (row[2] > 0) - b = 2; - else if (row[2] < 0) - b = 6; - else - b = 7; // error - return b; -} - -unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) -{ - unsigned short scalar; - /* - XYZ 010_001_000 Identity Matrix - XZY 001_010_000 - YXZ 010_000_001 - YZX 000_010_001 - ZXY 001_000_010 - ZYX 000_001_010 - */ - - scalar = inv_row_2_scale(mtx); - scalar |= inv_row_2_scale(mtx + 3) << 3; - scalar |= inv_row_2_scale(mtx + 6) << 6; - - return scalar; -} - -/* Setups up the Freescale 16-bit accel for Sensor Fusion -* @param[in] orient A scalar representation of the orientation -* that describes how to go from the Chip Orientation -* to the Board Orientation often times called the Body Orientation in Invensense Documentation. -* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. -*/ -inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient) -{ - unsigned char rr[3]; - inv_error_t result; - - orient = orient & 0xdb; - switch (orient) { - default: - // Typically 0x88 - rr[0] = DINACC; - rr[1] = DINACF; - rr[2] = DINA0E; - break; - case 0x50: - rr[0] = DINACE; - rr[1] = DINA0E; - rr[2] = DINACD; - break; - case 0x81: - rr[0] = DINACE; - rr[1] = DINACB; - rr[2] = DINA0E; - break; - case 0x11: - rr[0] = DINACC; - rr[1] = DINA0E; - rr[2] = DINACB; - break; - case 0x42: - rr[0] = DINA0A; - rr[1] = DINACF; - rr[2] = DINACB; - break; - case 0x0a: - rr[0] = DINA0A; - rr[1] = DINACB; - rr[2] = DINACD; - break; - } - result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr); - return result; -} - -/* Setups up the Freescale 8-bit accel for Sensor Fusion -* @param[in] orient A scalar representation of the orientation -* that describes how to go from the Chip Orientation -* to the Board Orientation often times called the Body Orientation in Invensense Documentation. -* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. -*/ -inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient) -{ - unsigned char regs[27]; - inv_error_t result; - uint_fast8_t kk; - - orient = orient & 0xdb; - kk = 0; - - regs[kk++] = DINAC3; - regs[kk++] = DINA90 + 14; - regs[kk++] = DINAA0 + 9; - regs[kk++] = DINA3E; - regs[kk++] = DINA5E; - regs[kk++] = DINA7E; - - regs[kk++] = DINAC2; - regs[kk++] = DINAA0 + 9; - regs[kk++] = DINA90 + 9; - regs[kk++] = DINAF8 + 2; - - switch (orient) { - default: - // Typically 0x88 - regs[kk++] = DINACB; - - regs[kk++] = DINA54; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - - regs[kk++] = DINACD; - break; - case 0x50: - regs[kk++] = DINACB; - - regs[kk++] = DINACF; - - regs[kk++] = DINA7C; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - break; - case 0x81: - regs[kk++] = DINA2C; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - - regs[kk++] = DINACD; - - regs[kk++] = DINACB; - break; - case 0x11: - regs[kk++] = DINA2C; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINA28; - regs[kk++] = DINACB; - regs[kk++] = DINACF; - break; - case 0x42: - regs[kk++] = DINACF; - regs[kk++] = DINACD; - - regs[kk++] = DINA7C; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - regs[kk++] = DINA78; - break; - case 0x0a: - regs[kk++] = DINACD; - - regs[kk++] = DINA54; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - regs[kk++] = DINA50; - - regs[kk++] = DINACF; - break; - } - - regs[kk++] = DINA90 + 7; - regs[kk++] = DINAF8 + 3; - regs[kk++] = DINAA0 + 9; - regs[kk++] = DINA0E; - regs[kk++] = DINA0E; - regs[kk++] = DINA0E; - - regs[kk++] = DINAF8 + 1; // filler - - result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/** - * Controlls each sensor and each axis when the motion processing unit is - * running. When it is not running, simply records the state for later. - * - * NOTE: In this version only full sensors controll is allowed. Independent - * axis control will return an error. - * - * @param sensors Bit field of each axis desired to be turned on or off - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_set_mpu_sensors(unsigned long sensors) -{ - INVENSENSE_FUNC_START; - unsigned char state = inv_get_state(); - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - inv_error_t result = INV_SUCCESS; - unsigned short fifoRate; - - if (state < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) && - ((sensors & INV_THREE_AXIS_ACCEL) != 0)) { - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - } - if (((sensors & INV_THREE_AXIS_ACCEL) != 0) && - (mldl_cfg->pdata->accel.get_slave_descr == 0)) { - return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; - } - - if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) && - ((sensors & INV_THREE_AXIS_COMPASS) != 0)) { - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - } - if (((sensors & INV_THREE_AXIS_COMPASS) != 0) && - (mldl_cfg->pdata->compass.get_slave_descr == 0)) { - return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; - } - - if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) && - ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) { - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - } - if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) && - (mldl_cfg->pdata->pressure.get_slave_descr == 0)) { - return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; - } - - /* DMP was off, and is turning on */ - if (sensors & INV_DMP_PROCESSOR && - !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { - struct ext_slave_config config; - long odr; - config.key = MPU_SLAVE_CONFIG_ODR_RESUME; - config.len = sizeof(long); - config.apply = (state == INV_STATE_DMP_STARTED); - config.data = &odr; - - odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000); - result = inv_mpu_config_accel(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; - odr = MPU_SLAVE_IRQ_TYPE_NONE; - result = inv_mpu_config_accel(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - inv_init_fifo_hardare(); - } - - if (inv_obj.mode_change_func) { - result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - /* Get the fifo rate before changing sensors so if we need to match it */ - fifoRate = inv_get_fifo_rate(); - mldl_cfg->requested_sensors = sensors; - - /* inv_dmp_start will turn the sensors on */ - if (state == INV_STATE_DMP_STARTED) { - result = inv_dl_start(sensors); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_reset_motion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_dl_stop(~sensors); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - inv_set_fifo_rate(fifoRate); - - if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) { - struct ext_slave_config config; - long data; - - config.len = sizeof(long); - config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; - config.apply = (state == INV_STATE_DMP_STARTED); - config.data = &data; - data = MPU_SLAVE_IRQ_TYPE_DATA_READY; - result = inv_mpu_config_accel(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - return result; -} - -void inv_set_mode_change(inv_error_t(*mode_change_func) - (unsigned long, unsigned long)) -{ - inv_obj.mode_change_func = mode_change_func; -} - -/** -* Mantis setup -*/ -#ifdef CONFIG_MPU_SENSORS_MPU6050B1 -inv_error_t inv_set_mpu_6050_config() -{ - long temp; - inv_error_t result; - unsigned char big8[4]; - unsigned char atc[4]; - long s[3], s2[3]; - int kk; - struct mldl_cfg *mldlCfg = inv_get_dl_config(); - - result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), - 0x0d, 4, atc); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - temp = atc[3] & 0x3f; - if (temp >= 32) - temp = temp - 64; - temp = (temp << 21) | 0x100000; - temp += (1L << 29); - temp = -temp; - result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - for (kk = 0; kk < 3; ++kk) { - s[kk] = atc[kk] & 0x3f; - if (s[kk] > 32) - s[kk] = s[kk] - 64; - s[kk] *= 2516582L; - } - - for (kk = 0; kk < 3; ++kk) { - s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] + - mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] + - mldlCfg->pdata->orientation[kk * 3 + 2] * s[2]; - } - result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} -#endif - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/ml.h b/60xx/mlsdk/mllite/ml.h deleted file mode 100644 index 838cd49..0000000 --- a/60xx/mlsdk/mllite/ml.h +++ /dev/null @@ -1,596 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: ml.h 5653 2011-06-16 21:06:55Z nroyer $ - * - *****************************************************************************/ - -/** - * @defgroup ML - * @brief The Motion Library processes gyroscopes and accelerometers to - * provide a physical model of the movement of the sensors. - * The results of this processing may be used to control objects - * within a user interface environment, detect gestures, track 3D - * movement for gaming applications, and analyze the blur created - * due to hand movement while taking a picture. - * - * @{ - * @file ml.h - * @brief Header file for the Motion Library. -**/ - -#ifndef INV_H -#define INV_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "mldmp.h" -#include "mlsl.h" -#include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "ml_legacy.h" -#endif - -/* ------------ */ -/* - Defines. - */ -/* ------------ */ - -/* - Module defines. - */ - -/*************************************************************************/ -/* Motion Library Vesion */ -/*************************************************************************/ - -#define INV_VERSION_MAJOR 4 -#define INV_VERSION_MINOR 0 -#define INV_VERSION_SUB_MINOR 0 - -#define INV_VERSION_MAJOR_STR "4" -#define INV_VERSION_MINOR_STR "0" -#define INV_VERSION_SUB_MINOR_STR "0" - -#define INV_VERSION_NONE "" -#define INV_VERSION_PROTOTYPE "ProtoA " -#define INV_VERSION_ENGINEERING "EngA " -#define INV_VERSION_PRE_ALPHA "Pre-Alpha " -#define INV_VERSION_ALPHA "Alpha " -#define INV_VERSION_BETA "Beta " -#define INV_VERSION_PRODUCTION "Production " - -#ifndef INV_VERSION_TYPE -#define INV_VERSION_TYPE INV_VERSION_NONE -#endif - -#define INV_VERSION "InvenSense MPL" " " \ - "v" INV_VERSION_MAJOR_STR "." INV_VERSION_MINOR_STR "." INV_VERSION_SUB_MINOR_STR " " \ - INV_VERSION_TYPE \ - __DATE__ " " __TIME__ - -/*************************************************************************/ -/* Motion processing engines */ -/*************************************************************************/ -#define INV_MOTION_DETECT (0x0004) -#define INV_BIAS_UPDATE (0x0008) -#define INV_GESTURE (0x0020) -#define INV_CONTROL (0x0040) -#define INV_ORIENTATION (0x0100) -#define INV_PEDOMETER (0x0200) -#define INV_BASIC (INV_MOTION_DETECT | INV_BIAS_UPDATE) - -/*************************************************************************/ -/* Data Source - Obsolete */ -/*************************************************************************/ -#define INV_DATA_FIFO (0x1) -#define INV_DATA_POLL (0x2) - -/*************************************************************************/ -/* Interrupt Source */ -/*************************************************************************/ -#define INV_INT_MOTION (0x01) -#define INV_INT_FIFO (0x02) -#define INV_INT_TAP (0x04) -#define INV_INT_ORIENTATION (0x08) -#define INV_INT_SHAKE_PITCH (0x10) -#define INV_INT_SHAKE_ROLL (0x20) -#define INV_INT_SHAKE_YAW (0x40) - -/*************************************************************************/ -/* Bias update functions */ -/*************************************************************************/ -#define INV_BIAS_FROM_NO_MOTION 0x0001 -#define INV_BIAS_FROM_GRAVITY 0x0002 -#define INV_BIAS_FROM_TEMPERATURE 0x0004 -#define INV_BIAS_FROM_LPF 0x0008 -#define INV_MAG_BIAS_FROM_MOTION 0x0010 -#define INV_MAG_BIAS_FROM_GYRO 0x0020 -#define INV_LEARN_BIAS_FROM_TEMPERATURE 0x0040 -#define INV_AUTO_RESET_MAG_BIAS 0x0080 -#define INV_REJECT_MAG_DISTURBANCE 0x0100 -#define INV_PROGRESSIVE_NO_MOTION 0x0200 -#define INV_BIAS_FROM_FAST_NO_MOTION 0x0400 - -/*************************************************************************/ -/* Euler angles and axis names */ -/*************************************************************************/ -#define INV_X_AXIS (0x01) -#define INV_Y_AXIS (0x02) -#define INV_Z_AXIS (0x04) - -#define INV_PITCH (INV_X_AXIS) -#define INV_ROLL (INV_Y_AXIS) -#define INV_YAW (INV_Z_AXIS) - -/*************************************************************************/ -/* Sensor types */ -/*************************************************************************/ -#define INV_GYROS 0x0001 -#define INV_ACCELS 0x0002 - -/*************************************************************************/ -/* Motion arrays */ -/*************************************************************************/ -#define INV_ROTATION_MATRIX 0x0003 -#define INV_QUATERNION 0x0004 -#define INV_EULER_ANGLES 0x0005 -#define INV_LINEAR_ACCELERATION 0x0006 -#define INV_LINEAR_ACCELERATION_WORLD 0x0007 -#define INV_GRAVITY 0x0008 -#define INV_ANGULAR_VELOCITY 0x0009 - -#define INV_GYRO_CALIBRATION_MATRIX 0x000B -#define INV_ACCEL_CALIBRATION_MATRIX 0x000C -#define INV_GYRO_BIAS 0x000D -#define INV_ACCEL_BIAS 0x000E -#define INV_GYRO_TEMP_SLOPE 0x000F - -#define INV_RAW_DATA 0x0011 -#define INV_DMP_TAP 0x0012 -#define INV_DMP_TAP2 0x0021 - -#define INV_EULER_ANGLES_X 0x0013 -#define INV_EULER_ANGLES_Y 0x0014 -#define INV_EULER_ANGLES_Z 0x0015 - -#define INV_BIAS_UNCERTAINTY 0x0016 -#define INV_DMP_PACKET_NUMBER 0x0017 -#define INV_FOOTER 0x0018 - -#define INV_CONTROL_DATA 0x0019 - -#define INV_MAGNETOMETER 0x001A -#define INV_PEDLBS 0x001B -#define INV_MAG_RAW_DATA 0x001C -#define INV_MAG_CALIBRATION_MATRIX 0x001D -#define INV_MAG_BIAS 0x001E -#define INV_HEADING 0x001F - -#define INV_MAG_BIAS_ERROR 0x0020 - -#define INV_PRESSURE 0x0021 -#define INV_LOCAL_FIELD 0x0022 -#define INV_MAG_SCALE 0x0023 - -#define INV_RELATIVE_QUATERNION 0x0024 - -#define SET_QUATERNION 0x0001 -#define SET_GYROS 0x0002 -#define SET_LINEAR_ACCELERATION 0x0004 -#define SET_GRAVITY 0x0008 -#define SET_ACCELS 0x0010 -#define SET_TAP 0x0020 -#define SET_PEDLBS 0x0040 -#define SET_LINEAR_ACCELERATION_WORLD 0x0080 -#define SET_CONTROL 0x0100 -#define SET_PACKET_NUMBER 0x4000 -#define SET_FOOTER 0x8000 - -/*************************************************************************/ -/* Integral reset options */ -/*************************************************************************/ -#define INV_NO_RESET 0x0000 -#define INV_RESET 0x0001 - -/*************************************************************************/ -/* Motion states */ -/*************************************************************************/ -#define INV_MOTION 0x0001 -#define INV_NO_MOTION 0x0002 - -/*************************************************************************/ -/* Orientation and Gesture states */ -/*************************************************************************/ -#define INV_STATE_IDLE (0) -#define INV_STATE_RUNNING (1) - -/*************************************************************************/ -/* Gyroscope Temperature Compensation bins */ -/*************************************************************************/ -#define BINS (25) -#define PTS_PER_BIN (5) -#define MIN_TEMP (-40) -#define MAX_TEMP (+85) -#define TEMP_PER_BIN ((MAX_TEMP - MIN_TEMP) / BINS) - -/*************************************************************************/ -/* Flags */ -/*************************************************************************/ -#define INV_RAW_DATA_READY 0x0001 -#define INV_PROCESSED_DATA_READY 0x0002 - -#define INV_GOT_GESTURE 0x0004 - -#define INV_MOTION_STATE_CHANGE 0x0006 -#define INV_COMPASS_OFFSET_VALID 0x0007 - -/*************************************************************************/ -/* General */ -/*************************************************************************/ -#define INV_NONE (0x0000) -#define INV_INVALID_FIFO_RATE (0xFFFF) - -/*************************************************************************/ -/* ML Params Structure Default Values */ -/*************************************************************************/ -#define INV_BIAS_UPDATE_FUNC_DEFAULT (INV_BIAS_FROM_NO_MOTION | INV_BIAS_FROM_GRAVITY) -#define INV_ORIENTATION_MASK_DEFAULT 0x3f -#define INV_PROCESSED_DATA_CALLBACK_DEFAULT 0 -#define INV_ORIENTATION_CALLBACK_DEFAULT 0 -#define INV_MOTION_CALLBACK_DEFAULT 0 - -/* ------------ */ -/* - Defines. - */ -/* ------------ */ -/* Priority for various features */ -#define INV_PRIORITY_BUS_ACCEL 100 -#define INV_PRIORITY_EXTERNAL_SLAVE_MAG 110 -#define INV_PRIORITY_FAST_NO_MOTION 120 -#define INV_PRIORITY_BIAS_NO_MOTION 125 -#define INV_PRIORITY_SET_GYRO_BIASES 150 -#define INV_PRIORITY_TEMP_COMP 175 -#define INV_PRIORITY_CONTROL 200 -#define INV_PRIORITY_EIS 300 -#define INV_PRIORITY_ORIENTATION 400 -#define INV_PRIORITY_PEDOMETER_FULLPOWER 500 -#define INV_PRIORITY_NAVIGATION_PEDOMETER 600 -#define INV_PRIORITY_GESTURE 700 -#define INV_PRIORITY_GLYPH 800 - -#define MAX_INTERRUPT_PROCESSES 5 -/* Number of quantized accel samples */ -#define INV_MAX_NUM_ACCEL_SAMPLES (8) - -#define PRECISION 10000.f -#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) { \ - range.mantissa = (long)x; \ - range.fraction = (long)((float)(x-(long)x)*PRECISION); \ -} -#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) { \ - x = (float)(range.mantissa); \ - x += ((float)range.fraction/PRECISION); \ -} - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - -struct inv_obj_t { - //Calibration parameters - /* Raw sensor orientation */ - long gyro_bias[3]; - long accel_bias[3]; - long compass_bias[3]; - - /* Cached values after orietnation is applied */ - long scaled_gyro_bias[3]; - long scaled_accel_bias[3]; - long scaled_compass_bias[3]; - - long compass_scale[3]; - long compass_test_bias[3]; - long compass_test_scale[3]; - long compass_asa[3]; - long compass_offsets[3]; - - long compass_bias_error[3]; - - long got_no_motion_bias; - long got_compass_bias; - long compass_state; - long large_field; - long acc_state; - - long factory_temp_comp; - long got_coarse_heading; - long gyro_temp_bias[3]; - long prog_no_motion_bias[3]; - - long accel_cal[9]; - // Deprecated, used gyro_orient - long gyro_cal[GYRO_NUM_AXES * GYRO_NUM_AXES]; - long gyro_orient[GYRO_NUM_AXES * GYRO_NUM_AXES]; - long accel_sens; - long compass_cal[9]; - long gyro_sens; - long gyro_sf; - long temp_slope[GYRO_NUM_AXES]; - long compass_sens; - long temp_offset[GYRO_NUM_AXES]; - - int cal_loaded_flag; - - /* temperature compensation */ - float x_gyro_coef[3]; - float y_gyro_coef[3]; - float z_gyro_coef[3]; - float x_gyro_temp_data[BINS][PTS_PER_BIN]; - float y_gyro_temp_data[BINS][PTS_PER_BIN]; - float z_gyro_temp_data[BINS][PTS_PER_BIN]; - float temp_data[BINS][PTS_PER_BIN]; - int temp_ptrs[BINS]; - long temp_valid_data[BINS]; - - long compass_correction[4]; - long compass_correction_relative[4]; - long compass_disturb_correction[4]; - long compass_correction_offset[4]; - long relative_quat[4]; - long local_field[3]; - long new_local_field; - long sync_grav_body[3]; - int gyro_bias_err; - - double compass_bias_ptr[9]; - double compass_bias_v[3]; - double compass_prev_m[36]; - double compass_prev_xty[6]; - - int compass_peaks[18]; - int all_sensors_no_motion; - - long init_compass_bias[3]; - - int got_init_compass_bias; - int resetting_compass; - - long ang_v_body[GYRO_NUM_AXES]; - unsigned char compass_raw_data[24]; /* Sensor data plus status etc */ - long compass_sensor_data[3]; /* Raw sensor data only */ - long compass_calibrated_data[3]; - long compass_test_calibrated_data[3]; - long pressure; - inv_error_t (*external_slave_callback)(struct inv_obj_t *); - int compass_accuracy; - int compass_overunder; - - unsigned short flags[8]; - unsigned short suspend; - - long no_motion_threshold; - unsigned long motion_duration; - - unsigned short motion_state; - - unsigned short data_mode; - unsigned short interrupt_sources; - - unsigned short bias_update_time; - short last_motion; - unsigned short bias_calc_time; - - unsigned char internal_motion_state; - long start_time; - - long accel_lpf_gain; - long accel_lpf[3]; - unsigned long poll_no_motion; - long no_motion_accel_threshold; - unsigned long no_motion_accel_time; - inv_error_t(*mode_change_func) (unsigned long, unsigned long); - }; - - typedef inv_error_t(*inv_obj_func) (struct inv_obj_t *); - - extern struct inv_obj_t inv_obj; - - /* --------------------- */ - /* - Params Structure. - */ - /* --------------------- */ - - struct inv_params_obj { - - unsigned short bias_mode; // A function or bitwise OR of functions that determine how the gyroscope bias will be automatically updated. - // Functions include INV_BIAS_FROM_NO_MOTION, INV_BIAS_FROM_GRAVITY, and INV_BIAS_FROM_TEMPERATURE. - // The engine INV_BIAS_UPDATE must be enabled for these algorithms to run. - - unsigned short orientation_mask; // Allows a user to register which orientations will trigger the user defined callback function. - // The orientations are INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, and INV_Z_DOWN. - // INV_ORIENTATION_ALL is equivalent to INV_X_UP | INV_X_DOWN | INV_Y_UP | INV_Y_DOWN | INV_Z_UP | INV_Z_DOWN. - - void (*fifo_processed_func) (void); // Callback function that triggers when all the processing has been finished by the motion processing engines. - - void (*orientation_cb_func) (unsigned short orient); // Callback function that will run when a change of orientation is detected. - // The new orientation. May be one of INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, or INV_Z_DOWN. - - void (*motion_cb_func) (unsigned short motion_state); // Callback function that will run when a change of motion state is detected. - // The new motion state. May be one of INV_MOTION, or INV_NO_MOTION. - - unsigned char state; - - }; - - extern struct inv_params_obj inv_params_obj; - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - - inv_error_t inv_serial_start(char const *port); - inv_error_t inv_serial_stop(void); - inv_error_t inv_set_mpu_sensors(unsigned long sensors); - void *inv_get_serial_handle(void); - - /*API for handling the buffer */ - inv_error_t inv_update_data(void); - - /*API for handling polling */ - int inv_check_flag(int flag); - - /*API for setting bias update function */ - inv_error_t inv_set_bias_update(unsigned short biasFunction); - -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - inv_error_t inv_turn_on_bias_from_no_motion(void); - inv_error_t inv_turn_off_bias_from_no_motion(void); - inv_error_t inv_set_mpu_6050_config(void); -#endif - - /* Legacy functions for handling augmented data*/ - inv_error_t inv_get_array(int dataSet, long *data); - inv_error_t inv_get_float_array(int dataSet, float *data); - inv_error_t inv_set_array(int dataSet, long *data); - inv_error_t inv_set_float_array(int dataSet, float *data); - /* Individual functions for augmented data, per specific dataset */ - - - inv_error_t inv_get_gyro(long *data); - inv_error_t inv_get_accel(long *data); - inv_error_t inv_get_temperature(long *data); - inv_error_t inv_get_temperature_raw(short *data); - inv_error_t inv_get_rot_mat(long *data); - inv_error_t inv_get_quaternion(long *data); - inv_error_t inv_get_linear_accel(long *data); - inv_error_t inv_get_linear_accel_in_world(long *data); - inv_error_t inv_get_gravity(long *data); - inv_error_t inv_get_angular_velocity(long *data); - inv_error_t inv_get_euler_angles(long *data); - inv_error_t inv_get_euler_angles_x(long *data); - inv_error_t inv_get_euler_angles_y(long *data); - inv_error_t inv_get_euler_angles_z(long *data); - inv_error_t inv_get_gyro_temp_slope(long *data); - inv_error_t inv_get_gyro_bias(long *data); - inv_error_t inv_get_accel_bias(long *data); - inv_error_t inv_get_mag_bias(long *data); - inv_error_t inv_get_gyro_and_accel_sensor(long *data); - inv_error_t inv_get_mag_raw_data(long *data); - inv_error_t inv_get_magnetometer(long *data); - inv_error_t inv_get_pressure(long *data); - inv_error_t inv_get_heading(long *data); - inv_error_t inv_get_gyro_cal_matrix(long *data); - inv_error_t inv_get_accel_cal_matrix(long *data); - inv_error_t inv_get_mag_cal_matrix(long *data); - inv_error_t inv_get_mag_bias_error(long *data); - inv_error_t inv_get_mag_scale(long *data); - inv_error_t inv_get_local_field(long *data); - inv_error_t inv_get_relative_quaternion(long *data); - inv_error_t inv_get_gyro_float(float *data); - inv_error_t inv_get_accel_float(float *data); - inv_error_t inv_get_temperature_float(float *data); - inv_error_t inv_get_rot_mat_float(float *data); - inv_error_t inv_get_quaternion_float(float *data); - inv_error_t inv_get_linear_accel_float(float *data); - inv_error_t inv_get_linear_accel_in_world_float(float *data); - inv_error_t inv_get_gravity_float(float *data); - inv_error_t inv_get_angular_velocity_float(float *data); - inv_error_t inv_get_euler_angles_float(float *data); - inv_error_t inv_get_euler_angles_x_float(float *data); - inv_error_t inv_get_euler_angles_y_float(float *data); - inv_error_t inv_get_euler_angles_z_float(float *data); - inv_error_t inv_get_gyro_temp_slope_float(float *data); - inv_error_t inv_get_gyro_bias_float(float *data); - inv_error_t inv_get_accel_bias_float(float *data); - inv_error_t inv_get_mag_bias_float(float *data); - inv_error_t inv_get_gyro_and_accel_sensor_float(float *data); - inv_error_t inv_get_mag_raw_data_float(float *data); - inv_error_t inv_get_magnetometer_float(float *data); - inv_error_t inv_get_pressure_float(float *data); - inv_error_t inv_get_heading_float(float *data); - inv_error_t inv_get_gyro_cal_matrix_float(float *data); - inv_error_t inv_get_accel_cal_matrix_float(float *data); - inv_error_t inv_get_mag_cal_matrix_float(float *data); - inv_error_t inv_get_mag_bias_error_float(float *data); - inv_error_t inv_get_mag_scale_float(float *data); - inv_error_t inv_get_local_field_float(float *data); - inv_error_t inv_get_relative_quaternion_float(float *data); - inv_error_t inv_get_compass_accuracy(int *accuracy); - inv_error_t inv_set_gyro_bias(long *data); - inv_error_t inv_set_accel_bias(long *data); - inv_error_t inv_set_mag_bias(long *data); - inv_error_t inv_set_gyro_temp_slope(long *data); - inv_error_t inv_set_local_field(long *data); - inv_error_t inv_set_mag_scale(long *data); - inv_error_t inv_set_gyro_temp_slope_float(float *data); - inv_error_t inv_set_gyro_bias_float(float *data); - inv_error_t inv_set_accel_bias_float(float *data); - inv_error_t inv_set_mag_bias_float(float *data); - inv_error_t inv_set_local_field_float(float *data); - inv_error_t inv_set_mag_scale_float(float *data); - - inv_error_t inv_apply_endian_accel(void); - inv_error_t inv_apply_calibration(void); - inv_error_t inv_set_gyro_calibration(float range, signed char *orientation); - inv_error_t inv_set_accel_calibration(float range, - signed char *orientation); - inv_error_t inv_set_compass_calibration(float range, - signed char *orientation); - - /*API for detecting change of state */ - inv_error_t - inv_set_motion_callback(void (*func) (unsigned short motion_state)); - int inv_get_motion_state(void); - - /*API for getting ML version. */ - inv_error_t inv_get_version(unsigned char **version); - - inv_error_t inv_set_motion_interrupt(unsigned char on); - inv_error_t inv_set_fifo_interrupt(unsigned char on); - - int inv_get_interrupts(void); - - /* Simulated DMP */ - int inv_get_gyro_present(void); - - inv_error_t inv_set_no_motion_time(float time); - inv_error_t inv_set_no_motion_thresh(float thresh); - inv_error_t inv_set_no_motion_threshAccel(long thresh); - inv_error_t inv_reset_motion(void); - - inv_error_t inv_update_bias(void); - inv_error_t inv_set_dead_zone(void); - void inv_start_bias_calc(void); - void inv_stop_bias_calc(void); - - // Private functions shared accross modules - void inv_init_ml(void); - - inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func); - inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func); - void inv_run_dmp_interupt_cb(void); - void inv_set_mode_change(inv_error_t(*mode_change_func) - (unsigned long, unsigned long)); - -#ifdef __cplusplus -} -#endif -#endif // INV_H -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.c b/60xx/mlsdk/mllite/mlBiasNoMotion.c deleted file mode 100644 index 73321ff..0000000 --- a/60xx/mlsdk/mllite/mlBiasNoMotion.c +++ /dev/null @@ -1,393 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id:$ - * - *****************************************************************************/ - -#include "mlBiasNoMotion.h" -#include "ml.h" -#include "mlinclude.h" -#include "mlos.h" -#include "mlFIFO.h" -#include "dmpKey.h" -#include "accel.h" -#include "mlMathFunc.h" -#include "mldl.h" -#include "mlstates.h" -#include "mlSetGyroBias.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-BiasNoMot" - - -#define _mlDebug(x) //{x} - -/** - * @brief inv_set_motion_callback is used to register a callback function that - * will trigger when a change of motion state is detected. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() - * must <b>NOT</b> have been called. - * - * @param func A user defined callback function accepting a - * motion_state parameter, the new motion state. - * May be one of INV_MOTION or INV_NO_MOTION. - * @return INV_SUCCESS if successful or Non-zero error code otherwise. - */ -inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state)) -{ - INVENSENSE_FUNC_START; - - if ((inv_get_state() != INV_STATE_DMP_OPENED) && - (inv_get_state() != INV_STATE_DMP_STARTED)) - return INV_ERROR_SM_IMPROPER_STATE; - - inv_params_obj.motion_cb_func = func; - - return INV_SUCCESS; -} - -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 -/** Turns on the feature to compute gyro bias from No Motion */ -inv_error_t inv_turn_on_bias_from_no_motion() -{ - inv_error_t result; - unsigned char regs[3] = { 0x0d, DINA35, 0x5d }; - inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; - result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); - return result; -} - -/** Turns off the feature to compute gyro bias from No Motion -*/ -inv_error_t inv_turn_off_bias_from_no_motion() -{ - inv_error_t result; - unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 }; - inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; - result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); - return result; -} -#endif - -inv_error_t inv_update_bias(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - unsigned char regs[12]; - short bias[GYRO_NUM_AXES]; - - if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION) - && inv_get_gyro_present()) { - - regs[0] = DINAA0 + 3; - result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_get_mpu_memory(KEY_D_1_244, 12, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - inv_convert_bias(regs, bias); - - regs[0] = DINAA0 + 15; - result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = - inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), - MPUREG_TEMP_OUT_H, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - inv_obj.got_no_motion_bias = TRUE; - } - return INV_SUCCESS; -} - -inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) -{ - long gain; - unsigned long timeChange; - long rate; - inv_error_t result; - long accel[3], temp; - long long accelMag; - unsigned long currentTime; - int kk; - - if (!inv_accel_present()) { - return INV_SUCCESS; - } - - currentTime = inv_get_tick_count(); - - // We always run the accel low pass filter at the highest sample rate possible - result = inv_get_accel(accel); - if (result != INV_ERROR_FEATURE_NOT_ENABLED) { - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - rate = inv_get_fifo_rate() * 5 + 5; - if (rate > 200) - rate = 200; - - gain = inv_obj->accel_lpf_gain * rate; - timeChange = inv_get_fifo_rate(); - - accelMag = 0; - for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { - inv_obj->accel_lpf[kk] = - inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]); - inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]); - temp = accel[0] - inv_obj->accel_lpf[0]; - accelMag += (long long)temp *temp; - } - - if (accelMag > inv_obj->no_motion_accel_threshold) { - inv_obj->no_motion_accel_time = currentTime; - - // Check for change of state - if (!inv_get_gyro_present()) - inv_set_motion_state(INV_MOTION); - - } else if ((currentTime - inv_obj->no_motion_accel_time) > - 5 * inv_obj->motion_duration) { - // We have no motion according to accel - // Check fsor change of state - if (!inv_get_gyro_present()) - inv_set_motion_state(INV_NO_MOTION); - } - } - return INV_SUCCESS; -} - -/** - * @internal - * @brief Manually update the motion/no motion status. This is a - * convienence function for implementations that do not wish to use - * inv_update_data. - * This function can be called periodically to check for the - * 'no motion' state and update the internal motion status and bias - * calculations. - */ -inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) -{ - INVENSENSE_FUNC_START; - unsigned char regs[3] = { 0 }; - unsigned short motionFlag = 0; - unsigned long currentTime; - inv_error_t result; - - result = MLAccelMotionDetection(inv_obj); - - currentTime = inv_get_tick_count(); - - // If it is not time to poll for a no motion event, return - if (((inv_obj->interrupt_sources & INV_INT_MOTION) == 0) && - ((currentTime - inv_obj->poll_no_motion) <= 1000)) - return INV_SUCCESS; - - inv_obj->poll_no_motion = currentTime; - -#if defined CONFIG_MPU_SENSORS_MPU3050 - if (inv_get_gyro_present() - && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { - static long repeatBiasUpdateTime = 0; - - result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; - - _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); - ) - if (motionFlag == inv_obj->motion_duration) { - if (inv_obj->motion_state == INV_MOTION) { - inv_update_bias(); - repeatBiasUpdateTime = inv_get_tick_count(); - - regs[0] = DINAD8 + 1; - regs[1] = DINA0C; - regs[2] = DINAD8 + 2; - result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - regs[0] = 0; - regs[1] = 5; - result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - //Trigger no motion callback - inv_set_motion_state(INV_NO_MOTION); - } - } - if (motionFlag == 5) { - if (inv_obj->motion_state == INV_NO_MOTION) { - regs[0] = DINAD8 + 2; - regs[1] = DINA0C; - regs[2] = DINAD8 + 1; - result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - regs[0] = - (unsigned char)((inv_obj->motion_duration >> 8) & 0xff); - regs[1] = (unsigned char)(inv_obj->motion_duration & 0xff); - result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - //Trigger no motion callback - inv_set_motion_state(INV_MOTION); - } - } - if (inv_obj->motion_state == INV_NO_MOTION) { - if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) { - inv_update_bias(); - repeatBiasUpdateTime = inv_get_tick_count(); - } - } - } -#else // CONFIG_MPU_SENSORS_MPU3050 - if (inv_get_gyro_present() - && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { - result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; - - _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); - ) - if (motionFlag > 0) { - unsigned char biasReg[12]; - long biasTmp2[3], biasTmp[3]; - int i; - - if (inv_obj->last_motion != motionFlag) { - result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg); - - for (i = 0; i < 3; i++) { - biasTmp2[i] = inv_big8_to_int32(&biasReg[i * 4]); - } - // Rotate bias vector by the transpose of the orientation matrix - for (i = 0; i < 3; ++i) { - biasTmp[i] = - inv_q30_mult(biasTmp2[0], - inv_obj->gyro_orient[i]) + - inv_q30_mult(biasTmp2[1], - inv_obj->gyro_orient[i + 3]) + - inv_q30_mult(biasTmp2[2], inv_obj->gyro_orient[i + 6]); - } - inv_obj->gyro_bias[0] = inv_q30_mult(biasTmp[0], 1501974482L); - inv_obj->gyro_bias[1] = inv_q30_mult(biasTmp[1], 1501974482L); - inv_obj->gyro_bias[2] = inv_q30_mult(biasTmp[2], 1501974482L); - } - inv_set_motion_state(INV_NO_MOTION); - } else { - // We are in a motion state - inv_set_motion_state(INV_MOTION); - } - inv_obj->last_motion = motionFlag; - - } -#endif // CONFIG_MPU_SENSORS_MPU3050 - return INV_SUCCESS; -} - -inv_error_t inv_enable_bias_no_motion(void) -{ - inv_error_t result; - inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; - result = - inv_register_fifo_rate_process(MLPollMotionStatus, - INV_PRIORITY_BIAS_NO_MOTION); -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_turn_on_bias_from_no_motion(); -#endif - return result; -} - -inv_error_t inv_disable_bias_no_motion(void) -{ - inv_error_t result; - inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; - result = inv_unregister_fifo_rate_process(MLPollMotionStatus); -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_turn_off_bias_from_no_motion(); -#endif - return result; -} diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.h b/60xx/mlsdk/mllite/mlBiasNoMotion.h deleted file mode 100644 index 030dbf9..0000000 --- a/60xx/mlsdk/mllite/mlBiasNoMotion.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef INV_BIAS_NO_MOTION_H__ -#define INV_BIAS_NO_MOTION_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_bias_no_motion(void); - inv_error_t inv_disable_bias_no_motion(void); - -#ifdef __cplusplus -} -#endif -#endif // INV_BIAS_NO_MOTION_H__ diff --git a/60xx/mlsdk/mllite/mlFIFO.c b/60xx/mlsdk/mllite/mlFIFO.c deleted file mode 100644 index 2c0d2f0..0000000 --- a/60xx/mlsdk/mllite/mlFIFO.c +++ /dev/null @@ -1,2265 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $ - * - *******************************************************************************/ - -/** - * @defgroup MLFIFO - * @brief Motion Library - FIFO Driver. - * The FIFO API Interface. - * - * @{ - * @file mlFIFO.c - * @brief FIFO Interface. -**/ - -#include <string.h> -#include "mpu.h" -#if defined CONFIG_MPU_SENSORS_MPU6050A2 -# include "mpu6050a2.h" -#elif defined CONFIG_MPU_SENSORS_MPU6050B1 -# include "mpu6050b1.h" -#elif defined CONFIG_MPU_SENSORS_MPU3050 -# include "mpu3050.h" -#else -#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx -#endif -#include "mlFIFO.h" -#include "mlFIFOHW.h" -#include "dmpKey.h" -#include "mlMathFunc.h" -#include "ml.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mlstates.h" -#include "mlsupervisor.h" -#include "mlos.h" -#include "mlmath.h" -#include "accel.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-fifo" - -#define FIFO_DEBUG 0 - -#define REF_QUATERNION (0) -#define REF_GYROS (REF_QUATERNION + 4) -#define REF_CONTROL (REF_GYROS + 3) -#define REF_RAW (REF_CONTROL + 4) -#define REF_RAW_EXTERNAL (REF_RAW + 8) -#define REF_ACCEL (REF_RAW_EXTERNAL + 6) -#define REF_QUANT_ACCEL (REF_ACCEL + 3) -#define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) -#define REF_EIS (REF_QUATERNION_6AXIS + 4) -#define REF_DMP_PACKET (REF_EIS + 3) -#define REF_GARBAGE (REF_DMP_PACKET + 1) -#define REF_LAST (REF_GARBAGE + 1) - -long fifo_scale[REF_LAST] = { - (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion - // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2) - 1501974482L, 1501974482L, 1501974482L, // Gyro - (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control - (1L << 14), // Temperature - (1L << 14), (1L << 14), (1L << 14), // Raw Gyro - (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding - (1L << 14), (1L << 14), (1L << 14), // Raw External - (1L << 14), (1L << 14), (1L << 14), // Raw External - (1L << 16), (1L << 16), (1L << 16), // Accel - (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel - (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel - (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis - (1L << 30), (1L << 30), (1L << 30), // EIS - (1L << 30), // Packet - (1L << 30), // Garbage -}; - -// The scale factors for tap need to match the number in fifo_scale above. -// fifo_base_offset below may also need to be changed if this is not 8 -#if INV_MAX_NUM_ACCEL_SAMPLES != 8 -#error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8 -#endif - -#define CONFIG_QUAT (0) -#define CONFIG_GYROS (CONFIG_QUAT + 1) -#define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1) -#define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1) -#define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1) -#define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1) -#define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1) -#define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1) -#define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1) -#define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1) -#define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1) -#define NUMFIFOELEMENTS (CONFIG_FOOTER + 1) - -const int fifo_base_offset[NUMFIFOELEMENTS] = { - REF_QUATERNION * 4, - REF_GYROS * 4, - REF_CONTROL * 4, - REF_RAW * 4, - REF_RAW * 4 + 4, - REF_RAW_EXTERNAL * 4, - REF_ACCEL * 4, - REF_QUANT_ACCEL * 4, - REF_EIS * 4, - REF_DMP_PACKET * 4, - REF_GARBAGE * 4 -}; - -struct fifo_obj { - void (*fifo_process_cb) (void); - long decoded[REF_LAST]; - long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES]; - int offsets[REF_LAST * 4]; - int cache; - uint_fast8_t gyro_source; - unsigned short fifo_rate; - unsigned short sample_step_size_ms; - uint_fast16_t fifo_packet_size; - uint_fast16_t data_config[NUMFIFOELEMENTS]; - unsigned char reference_count[REF_LAST]; - long acc_bias_filt[3]; - float acc_filter_coef; - long gravity_cache[3]; -}; - -static struct fifo_obj fifo_obj; - -#define FIFO_CACHE_TEMPERATURE 1 -#define FIFO_CACHE_GYRO 2 -#define FIFO_CACHE_GRAVITY_BODY 4 -#define FIFO_CACHE_ACC_BIAS 8 - -struct fifo_rate_obj { - // These describe callbacks happening everytime a FIFO block is processed - int_fast8_t num_cb; - HANDLE mutex; - inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES]; - int priority[MAX_HIGH_RATE_PROCESSES]; -}; - -struct fifo_rate_obj fifo_rate_obj; - -/** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old - * accuracy if needed. - */ -static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements, - uint_fast16_t accuracy, - uint_fast8_t configOffset) -{ - if (elements) { - if (!accuracy) - accuracy = fifo_obj.data_config[configOffset]; - else if (accuracy & INV_16_BIT) - if ((fifo_obj.data_config[configOffset] & INV_32_BIT)) - accuracy = INV_32_BIT; // 32-bits takes priority - else - accuracy = INV_16_BIT; - else - accuracy = INV_32_BIT; - } else { - accuracy = 0; - } - - return accuracy; -} - -/** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, - * the reference counts are subtracted, otherwise they are incremented for each - * bit set in element. The value returned are the elements that should be sent - * out as data through the FIFO. -*/ -static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements, - uint_fast16_t increment, - uint_fast8_t refOffset, - uint_fast8_t len) -{ - uint_fast8_t kk; - - if (increment == 0) { - for (kk = 0; kk < len; ++kk) { - if ((elements & 1) - && (fifo_obj.reference_count[kk + refOffset] > 0)) { - fifo_obj.reference_count[kk + refOffset]--; - } - elements >>= 1; - } - } else { - for (kk = 0; kk < len; ++kk) { - if (elements & 1) - fifo_obj.reference_count[kk + refOffset]++; - elements >>= 1; - } - } - elements = 0; - for (kk = 0; kk < len; ++kk) { - if (fifo_obj.reference_count[kk + refOffset] > 0) - elements |= (1 << kk); - } - return elements; -} - -/** - * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send - * out the FIFO, 0 when removing from the FIFO. - */ -static inv_error_t inv_construct3_fifo(unsigned char *regs, - uint_fast16_t elements, - uint_fast16_t accuracy, - uint_fast8_t refOffset, - unsigned short key, - uint_fast8_t configOffset) -{ - int_fast8_t kk; - inv_error_t result; - - elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3); - accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset); - - if (accuracy & INV_16_BIT) { - regs[0] = DINAF8 + 2; - } - - fifo_obj.data_config[configOffset] = elements | accuracy; - - for (kk = 0; kk < 3; ++kk) { - if ((elements & 1) == 0) - regs[kk + 1] = DINAA0 + 3; - elements >>= 1; - } - - result = inv_set_mpu_memory(key, 4, regs); - - return result; -} - -/** - * @internal - * Puts footer on FIFO data. - */ -static inv_error_t inv_set_footer(void) -{ - unsigned char regs = DINA30; - uint_fast8_t tmp_count; - int_fast8_t i, j; - int offset; - int result; - int *fifo_offsets_ptr = fifo_obj.offsets; - - fifo_obj.fifo_packet_size = 0; - for (i = 0; i < NUMFIFOELEMENTS; i++) { - tmp_count = 0; - offset = fifo_base_offset[i]; - for (j = 0; j < 8; j++) { - if ((fifo_obj.data_config[i] >> j) & 0x0001) { -#ifndef BIG_ENDIAN - // Special Case for Byte Ordering on Accel Data - if ((i == CONFIG_RAW_DATA) && (j > 2)) { - tmp_count += 2; - switch (inv_get_dl_config()->accel->endian) { - case EXT_SLAVE_BIG_ENDIAN: - *fifo_offsets_ptr++ = offset + 3; - *fifo_offsets_ptr++ = offset + 2; - break; - case EXT_SLAVE_LITTLE_ENDIAN: - *fifo_offsets_ptr++ = offset + 2; - *fifo_offsets_ptr++ = offset + 3; - break; - case EXT_SLAVE_FS8_BIG_ENDIAN: - if (j == 3) { - // Throw this byte away - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - *fifo_offsets_ptr++ = offset + 3; - } else if (j == 4) { - *fifo_offsets_ptr++ = offset + 3; - *fifo_offsets_ptr++ = offset + 7; - } else { - // Throw these byte away - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - } - break; - case EXT_SLAVE_FS16_BIG_ENDIAN: - if (j == 3) { - // Throw this byte away - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - *fifo_offsets_ptr++ = offset + 3; - } else if (j == 4) { - *fifo_offsets_ptr++ = offset - 2; - *fifo_offsets_ptr++ = offset + 3; - } else { - *fifo_offsets_ptr++ = offset - 2; - *fifo_offsets_ptr++ = offset + 3; - } - break; - default: - return INV_ERROR; // Bad value on ordering - } - } else { - tmp_count += 2; - *fifo_offsets_ptr++ = offset + 3; - *fifo_offsets_ptr++ = offset + 2; - if (fifo_obj.data_config[i] & INV_32_BIT) { - *fifo_offsets_ptr++ = offset + 1; - *fifo_offsets_ptr++ = offset; - tmp_count += 2; - } - } -#else - // Big Endian Platform - // Special Case for Byte Ordering on Accel Data - if ((i == CONFIG_RAW_DATA) && (j > 2)) { - tmp_count += 2; - switch (inv_get_dl_config()->accel->endian) { - case EXT_SLAVE_BIG_ENDIAN: - *fifo_offsets_ptr++ = offset + 2; - *fifo_offsets_ptr++ = offset + 3; - break; - case EXT_SLAVE_LITTLE_ENDIAN: - *fifo_offsets_ptr++ = offset + 3; - *fifo_offsets_ptr++ = offset + 2; - break; - case EXT_SLAVE_FS8_BIG_ENDIAN: - if (j == 3) { - // Throw this byte away - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - *fifo_offsets_ptr++ = offset; - } else if (j == 4) { - *fifo_offsets_ptr++ = offset; - *fifo_offsets_ptr++ = offset + 4; - } else { - // Throw these bytes away - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - } - break; - case EXT_SLAVE_FS16_BIG_ENDIAN: - if (j == 3) { - // Throw this byte away - *fifo_offsets_ptr++ = - fifo_base_offset[CONFIG_FOOTER]; - *fifo_offsets_ptr++ = offset; - } else if (j == 4) { - *fifo_offsets_ptr++ = offset - 3; - *fifo_offsets_ptr++ = offset; - } else { - *fifo_offsets_ptr++ = offset - 3; - *fifo_offsets_ptr++ = offset; - } - break; - default: - return INV_ERROR; // Bad value on ordering - } - } else { - tmp_count += 2; - *fifo_offsets_ptr++ = offset; - *fifo_offsets_ptr++ = offset + 1; - if (fifo_obj.data_config[i] & INV_32_BIT) { - *fifo_offsets_ptr++ = offset + 2; - *fifo_offsets_ptr++ = offset + 3; - tmp_count += 2; - } - } - -#endif - } - offset += 4; - } - fifo_obj.fifo_packet_size += tmp_count; - } - if (fifo_obj.data_config[CONFIG_FOOTER] == 0 && - fifo_obj.fifo_packet_size > 0) { - // Add footer - result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT; - fifo_obj.fifo_packet_size += 2; - } else if (fifo_obj.data_config[CONFIG_FOOTER] && - (fifo_obj.fifo_packet_size == 2)) { - // Remove Footer - regs = DINAA0 + 3; - result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - fifo_obj.data_config[CONFIG_FOOTER] = 0; - fifo_obj.fifo_packet_size = 0; - } - - return INV_SUCCESS; -} - -inv_error_t inv_decode_quantized_accel(void) -{ - int kk; - int fifoRate = inv_get_fifo_rate(); - - if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1)); - kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) { - union { - unsigned int u10:10; - signed int s10:10; - } temp; - - union { - uint32_t u32; - int32_t s32; - } value; - - value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk]; - // unquantize this samples. - // They are stored as x * 2^20 + y * 2^10 + z - // Z - temp.u10 = value.u32 & 0x3ff; - value.s32 -= temp.s10; - fifo_obj.decoded_accel[kk][2] = temp.s10 * 64; - // Y - value.s32 = value.s32 / 1024; - temp.u10 = value.u32 & 0x3ff; - value.s32 -= temp.s10; - fifo_obj.decoded_accel[kk][1] = temp.s10 * 64; - // X - value.s32 = value.s32 / 1024; - temp.u10 = value.u32 & 0x3ff; - fifo_obj.decoded_accel[kk][0] = temp.s10 * 64; - } - return INV_SUCCESS; -} - -static inv_error_t inv_state_change_fifo(unsigned char newState) -{ - inv_error_t result = INV_SUCCESS; - unsigned char regs[4]; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - /* Don't reset the fifo on a fifo rate change */ - if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && - (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) { - /* Delay output on restart by 50ms due to warm up time of gyros */ - - short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1); - inv_init_fifo_hardare(); - inv_int16_to_big8(delay, regs); - result = inv_set_mpu_memory(KEY_D_1_178, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - if (INV_STATE_DMP_STARTED == newState) { - if (inv_dmpkey_supported(KEY_D_1_128)) { - double tmp; - tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1); - if (tmp > 0x40000000L) - tmp = 0x40000000L; - (void)inv_int32_to_big8((long)tmp, regs); - result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_reset_fifo(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - return result; -} - -/** - * @internal - * @brief get the FIFO packet size - * @return the FIFO packet size - */ -uint_fast16_t inv_get_fifo_packet_size(void) -{ - return fifo_obj.fifo_packet_size; -} - -/** - * @brief Initializes all the internal static variables for - * the FIFO module. - * @note Should be called by the initialization routine such - * as inv_dmp_open(). - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_init_fifo_param(void) -{ - inv_error_t result; - memset(&fifo_obj, 0, sizeof(struct fifo_obj)); - fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity - inv_set_linear_accel_filter_coef(0.f); - fifo_obj.fifo_rate = 20; - fifo_obj.sample_step_size_ms = 100; - memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); - result = inv_create_mutex(&fifo_rate_obj.mutex); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_register_state_callback(inv_state_change_fifo); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief Close the FIFO usage. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_close_fifo(void) -{ - inv_error_t result; - inv_error_t first = INV_SUCCESS; - result = inv_unregister_state_callback(inv_state_change_fifo); - ERROR_CHECK_FIRST(first, result); - result = inv_destroy_mutex(fifo_rate_obj.mutex); - ERROR_CHECK_FIRST(first, result); - memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); - return first; -} - -/** - * Set the gyro source to output to the fifo - * - * @param source The source. One of - * - INV_GYRO_FROM_RAW - * - INV_GYRO_FROM_QUATERNION - * - * @return INV_SUCCESS or non-zero error code; - */ -inv_error_t inv_set_gyro_data_source(uint_fast8_t source) -{ - if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) { - return INV_ERROR_INVALID_PARAMETER; - } - - fifo_obj.gyro_source = source; - return INV_SUCCESS; -} - -/** - * @brief Reads and processes FIFO data. Also handles callbacks when data is - * ready. - * @param numPackets - * Number of FIFO packets to try to read. You should - * use a large number here, such as 100, if you want to read all - * the full packets in the FIFO, which is typical operation. - * @param processed - * The number of FIFO packets processed. This may be incremented - * even if high rate processes later fail. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, - int_fast8_t * processed) -{ - int_fast8_t packet; - inv_error_t result = INV_SUCCESS; - uint_fast16_t read; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - int kk; - - if (NULL == processed) - return INV_ERROR_INVALID_PARAMETER; - - *processed = 0; - if (fifo_obj.fifo_packet_size == 0) - return result; // Nothing to read - - for (packet = 0; packet < numPackets; ++packet) { - if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { - unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE]; - unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE]; - read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size, - footer_n_data); - if (0 == read || - read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) { - result = inv_get_fifo_status(); - if (INV_SUCCESS != result) { - memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); - } - return result; - } - - result = inv_process_fifo_packet(buf); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else if (inv_accel_present()) { - long data[ACCEL_NUM_AXES]; - result = inv_get_accel_data(data); - if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { - return INV_SUCCESS; - } - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); - fifo_obj.cache = 0; - for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { - fifo_obj.decoded[REF_RAW + 4 + kk] = - inv_q30_mult((data[kk] << 16), - fifo_scale[REF_RAW + 4 + kk]); - fifo_obj.decoded[REF_ACCEL + kk] = - inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]); - fifo_obj.decoded[REF_ACCEL + kk] -= - inv_obj.scaled_accel_bias[kk]; - } - } - // The buffer was processed correctly, so increment even if - // other processes fail later, which will return an error - *processed = *processed + 1; - - if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) && - fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) { - result = inv_decode_quantized_accel(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - if (fifo_obj.data_config[CONFIG_QUAT]) { - result = inv_accel_compass_supervisor(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - result = inv_pressure_supervisor(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - // Callbacks now that we have a buffer of data ready - result = inv_run_fifo_rate_processes(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - } - return result; -} - -/** - * @brief inv_set_fifo_processed_callback is used to set a processed data callback - * function. inv_set_fifo_processed_callback sets a user defined callback - * function that triggers when all the decoding has been finished by - * the motion processing engines. It is called before other bigger - * processing engines to allow lower latency for the user. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() - * must <b>NOT</b> have been called. - * - * @param func A user defined callback function. - * - * @return INV_SUCCESS if successful, or non-zero error code otherwise. - */ -inv_error_t inv_set_fifo_processed_callback(void (*func) (void)) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - fifo_obj.fifo_process_cb = func; - - return INV_SUCCESS; -} - -/** - * @internal - * @brief Process data from the dmp read via the fifo. Takes a buffer - * filled with bytes read from the DMP FIFO. - * Currently expects only the 16 bytes of quaternion data. - * Calculates the motion parameters from that data and stores the - * results in an internal data structure. - * @param[in,out] dmpData Pointer to the buffer containing the fifo data. - * @return INV_SUCCESS or error code. -**/ -inv_error_t inv_process_fifo_packet(const unsigned char *dmpData) -{ - INVENSENSE_FUNC_START; - int N, kk; - unsigned char *p; - - p = (unsigned char *)(&fifo_obj.decoded); - N = fifo_obj.fifo_packet_size; - if (N > sizeof(fifo_obj.decoded)) - return INV_ERROR_ASSERTION_FAILURE; - - memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); - - for (kk = 0; kk < N; ++kk) { - p[fifo_obj.offsets[kk]] = *dmpData++; - } - - // If multiplies are much greater cost than if checks, you could check - // to see if fifo_scale is non-zero first, or equal to (1L<<30) - for (kk = 0; kk < REF_LAST; ++kk) { - fifo_obj.decoded[kk] = - inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]); - } - - memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS], - &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long)); - - inv_obj.flags[INV_PROCESSED_DATA_READY] = 1; - fifo_obj.cache = 0; - - return INV_SUCCESS; -} - -/** Converts 16-bit temperature data as read from temperature register -* into Celcius scaled by 2^16. -*/ -long inv_decode_temperature(short tempReg) -{ -#if defined CONFIG_MPU_SENSORS_MPU6050A2 - // Celcius = 35 + (T + 3048.7)/325.9 - return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L); -#endif -#if defined CONFIG_MPU_SENSORS_MPU6050B1 - // Celcius = 35 + (T + 927.4)/360.6 - return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L); -#endif -#if defined CONFIG_MPU_SENSORS_MPU3050 - // Celcius = 35 + (T + 13200)/280 - return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L); -#endif -} - -/** @internal -* Returns the temperature in hardware units. The scaling may change from part to part. -*/ -inv_error_t inv_get_temperature_raw(short *data) -{ - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) { - inv_error_t result; - unsigned char regs[2]; - if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) { - if (FIFO_DEBUG) - MPL_LOGI("Fetching the temperature from the registers\n"); - fifo_obj.cache |= FIFO_CACHE_TEMPERATURE; - result = inv_serial_read(inv_get_serial_handle(), - inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2, - regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]); - } - } - *data = (short)fifo_obj.decoded[REF_RAW]; - return INV_SUCCESS; -} - -/** - * @brief Returns 1-element vector of temperature. It is read from the hardware if it - * doesn't exist in the FIFO. - * @param[out] data 1-element vector of temperature - * @return 0 on success or an error code. - */ -inv_error_t inv_get_temperature(long *data) -{ - short tr; - inv_error_t result; - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - result = inv_get_temperature_raw(&tr); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - data[0] = inv_decode_temperature(tr); - return INV_SUCCESS; -} - -/** - * @brief Get the Decoded Accel Data. - * @param data - * a buffer to store the quantized data. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_get_unquantized_accel(long *data) -{ - int ii, kk; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { - for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { - data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk]; - } - } - - return INV_SUCCESS; -} - -/** - * @brief Get the Quantized Accel data algorithm output from the FIFO. - * @param data - * a buffer to store the quantized data. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_get_quantized_accel(long *data) -{ - int ii; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { - data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii]; - } - - return INV_SUCCESS; -} - -/** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO -* and it is read from the registers if it was not put into the FIFO. The data is -* cached till the next FIFO processing block time. -* @param[out] data Length 3, Gyro data -*/ -inv_error_t inv_get_gyro_sensor(long *data) -{ - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) { - inv_error_t result; - unsigned char regs[6]; - if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) { - fifo_obj.cache |= FIFO_CACHE_GYRO; - result = - inv_serial_read(inv_get_serial_handle(), - inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6, - regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - fifo_obj.decoded[REF_RAW + 1] = - (((long)regs[0]) << 24) | (((long)regs[1]) << 16); - fifo_obj.decoded[REF_RAW + 2] = - (((long)regs[2]) << 24) | (((long)regs[3]) << 16); - fifo_obj.decoded[REF_RAW + 3] = - (((long)regs[4]) << 24) | (((long)regs[5]) << 16); - - // Temperature starts at location 0, Gyro at location 1. - fifo_obj.decoded[REF_RAW + 1] = - inv_q30_mult(fifo_obj.decoded[REF_RAW + 1], - fifo_scale[REF_RAW + 1]); - fifo_obj.decoded[REF_RAW + 2] = - inv_q30_mult(fifo_obj.decoded[REF_RAW + 2], - fifo_scale[REF_RAW + 2]); - fifo_obj.decoded[REF_RAW + 3] = - inv_q30_mult(fifo_obj.decoded[REF_RAW + 3], - fifo_scale[REF_RAW + 3]); - } - data[0] = fifo_obj.decoded[REF_RAW + 1]; - data[1] = fifo_obj.decoded[REF_RAW + 2]; - data[2] = fifo_obj.decoded[REF_RAW + 3]; - } else { - long data2[6]; - inv_get_gyro_and_accel_sensor(data2); - data[0] = data2[0]; - data[1] = data2[1]; - data[2] = data2[2]; - } - return INV_SUCCESS; -} - -/** - * @brief Returns 6-element vector of gyro and accel data - * @param[out] data 6-element vector of gyro and accel data - * @return 0 on success or an error code. - */ -inv_error_t inv_get_gyro_and_accel_sensor(long *data) -{ - int ii; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_RAW_DATA]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) { - data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii]; - } - - return INV_SUCCESS; -} - -/** - * @brief Returns 3-element vector of external sensor - * @param[out] data 3-element vector of external sensor - * @return 0 on success or an error code. - */ -inv_error_t inv_get_external_sensor_data(long *data, int size) -{ -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - int ii; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (ii = 0; ii < size && ii < 6; ii++) { - data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii]; - } - - return INV_SUCCESS; -#else - memset(data, 0, COMPASS_NUM_AXES * sizeof(long)); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; -#endif -} - -/** - * Sends accelerometer data to the FIFO. - * - * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis - * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together - * for a subset. - * - * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. Set to zero to remove it from the FIFO. - */ -inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 }; - inv_error_t result; - int kk; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL, - KEY_CFG_12, CONFIG_ACCEL); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { - fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens; - } - - return inv_set_footer(); -} - -/** - * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits. - * - * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all - * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd - * together for a subset. - * - * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. Set to zero to remove it from the FIFO. - */ -inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - int_fast8_t kk; - inv_error_t result; - unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4); - accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA); - - if (accuracy & INV_16_BIT) { - regs[0] = DINAF8 + 2; - } - - fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy; - - for (kk = 0; kk < 4; ++kk) { - if ((elements & 1) == 0) - regs[kk + 1] = DINAA0 + 3; - elements >>= 1; - } - - result = inv_set_mpu_memory(KEY_CFG_1, 5, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); -} - -/** - * Adds a rolling counter to the fifo packet. When used with the footer - * the data comes out the first time: - * - * @code - * <data0><data1>...<dataN><PacketNum0><PacketNum1> - * @endcode - * for every other packet it is - * - * @code - * <FifoFooter0><FifoFooter1><data0><data1>...<dataN><PacketNum0><PacketNum1> - * @endcode - * - * This allows for scanning of the fifo for packets - * - * @return INV_SUCCESS or error code - */ -inv_error_t inv_send_packet_number(uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - unsigned char regs; - uint_fast16_t elements; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1); - if (elements & 1) { - regs = DINA28; - fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = - INV_ELEMENT_1 | INV_16_BIT; - } else { - regs = DINAF8 + 3; - fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0; - } - result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); -} - -/** - * @brief Send the computed gravity vectors into the FIFO. - * The gravity vectors can be retrieved from the FIFO via - * inv_get_gravity(), to have the gravitation vector expressed - * in coordinates relative to the body. - * - * Gravity is a derived vector derived from the quaternion. - * @param elements - * the gravitation vectors components bitmask. - * To send all compoents use INV_ALL. - * @param accuracy - * The number of bits the gravitation vector is expressed - * into. - * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. - * Set to zero to remove it from the FIFO. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - result = inv_send_quaternion(accuracy); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); -} - -/** Sends gyro data to the FIFO. Gyro data is a 3 length vector - * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). - * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them - * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together - * for a subset. - * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. Set to zero to remove it from the FIFO. - */ -inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 }; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) { - regs[0] = DINA90 + 5; - result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - regs[0] = DINAF8 + 1; - regs[1] = DINA20; - regs[2] = DINA28; - regs[3] = DINA30; - } else { - regs[0] = DINA90 + 10; - result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - regs[0] = DINAF8 + 1; - regs[1] = DINA28; - regs[2] = DINA30; - regs[3] = DINA38; - } - result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS, - KEY_CFG_9, CONFIG_GYROS); - - return inv_set_footer(); -} - -/** Sends linear accelerometer data to the FIFO. - * - * Linear accelerometer data is a 3 length vector of 32-bits. It is the - * acceleration in the body frame with gravity removed. - * - * - * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of - * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together - * for a subset. - * - * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES - * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. Set to zero to remove it from the FIFO. - */ -inv_error_t inv_send_linear_accel(uint_fast16_t elements, - uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - unsigned char state = inv_get_state(); - - if (state < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - result = inv_send_gravity(elements, accuracy); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_send_accel(elements, accuracy); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); -} - -/** Sends linear world accelerometer data to the FIFO. Linear world - * accelerometer data is a 3 length vector of 32-bits. It is the acceleration - * in the world frame with gravity removed. Should be called once after - * inv_dmp_open() and before inv_dmp_start(). - * - * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of - * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together - * for a subset. - * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. - */ -inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, - uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - result = inv_send_linear_accel(INV_ALL, accuracy); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_send_quaternion(accuracy); - - return inv_set_footer(); -} - -/** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector - * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). - * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 - * bit data. - */ -inv_error_t inv_send_quaternion(uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, - DINA30, DINA38 - }; - uint_fast16_t elements, kk; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4); - accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT); - - if (accuracy & INV_16_BIT) { - regs[0] = DINAF8 + 2; - } - - fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy; - - for (kk = 0; kk < 4; ++kk) { - if ((elements & 1) == 0) - regs[kk + 1] = DINAA0 + 3; - elements >>= 1; - } - - result = inv_set_mpu_memory(KEY_CFG_8, 5, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); -} - -/** Sends raw data to the FIFO. - * Should be called once after inv_dmp_open() and before inv_dmp_start(). - * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them - * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together - * for a subset. The first element is temperature, the next 3 are gyro data, - * and the last 3 accel data. - * @param accuracy - * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off. - * @return 0 if successful, a non-zero error code otherwise. - */ -inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy) -{ - int result; -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, - DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, - DINAA0 + 3 - }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (accuracy) - accuracy = INV_16_BIT; - - elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); - - if (elements & 1) - fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; - else - fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; - if (elements & 0x7e) - fifo_obj.data_config[CONFIG_RAW_DATA] = - (0x3f & (elements >> 1)) | INV_16_BIT; - else - fifo_obj.data_config[CONFIG_RAW_DATA] = 0; - - if (elements & INV_ELEMENT_1) { - regs[0] = DINACA; - } - if (elements & INV_ELEMENT_2) { - regs[1] = DINBC4; - } - if (elements & INV_ELEMENT_3) { - regs[2] = DINACC; - } - if (elements & INV_ELEMENT_4) { - regs[3] = DINBC6; - } - if (elements & INV_ELEMENT_5) { - regs[4] = DINBC0; - } - if (elements & INV_ELEMENT_6) { - regs[5] = DINAC8; - } - if (elements & INV_ELEMENT_7) { - regs[6] = DINBC2; - } - result = inv_set_mpu_memory(KEY_CFG_15, 7, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); - -#else - INVENSENSE_FUNC_START; - unsigned char regs[4] = { DINAA0 + 3, - DINAA0 + 3, - DINAA0 + 3, - DINAA0 + 3 - }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (accuracy) - accuracy = INV_16_BIT; - - elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); - - if (elements & 0x03) { - elements |= 0x03; - regs[0] = DINA20; - } - if (elements & 0x0C) { - elements |= 0x0C; - regs[1] = DINA28; - } - if (elements & 0x30) { - elements |= 0x30; - regs[2] = DINA30; - } - if (elements & 0x40) { - elements |= 0xC0; - regs[3] = DINA38; - } - - result = inv_set_mpu_memory(KEY_CFG_15, 4, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (elements & 0x01) - fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; - else - fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; - if (elements & 0xfe) - fifo_obj.data_config[CONFIG_RAW_DATA] = - (0x7f & (elements >> 1)) | INV_16_BIT; - else - fifo_obj.data_config[CONFIG_RAW_DATA] = 0; - - return inv_set_footer(); -#endif -} - -/** Sends raw external data to the FIFO. - * Should be called once after inv_dmp_open() and before inv_dmp_start(). - * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them - * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together - * for a subset. - * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data. - * Sending and Stop sending are reference counted, so data actually - * stops when the reference reaches zero. - */ -inv_error_t inv_send_external_sensor_data(uint_fast16_t elements, - uint_fast16_t accuracy) -{ -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - int result; - unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3, - DINAA0 + 3, DINAA0 + 3, - DINAA0 + 3, DINAA0 + 3 }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (accuracy) - accuracy = INV_16_BIT; - - elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6); - - if (elements) - fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT; - else - fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0; - - if (elements & INV_ELEMENT_1) { - regs[0] = DINBC2; - } - if (elements & INV_ELEMENT_2) { - regs[1] = DINACA; - } - if (elements & INV_ELEMENT_3) { - regs[2] = DINBC4; - } - if (elements & INV_ELEMENT_4) { - regs[3] = DINBC0; - } - if (elements & INV_ELEMENT_5) { - regs[4] = DINAC8; - } - if (elements & INV_ELEMENT_6) { - regs[5] = DINACC; - } - - result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); - -#else - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported -#endif -} - -/** - * @brief Send the Quantized Acceleromter data into the FIFO. The data can be - * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel(). - * - * To be useful this should be set to fifo_rate + 1 if less than - * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work. - * - * @param elements - * the components bitmask. - * To send all compoents use INV_ALL. - * - * @param accuracy - * Use INV_32_BIT for 32-bit data or INV_16_BIT for - * 16-bit data. - * Set to zero to remove it from the FIFO. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_send_quantized_accel(uint_fast16_t elements, - uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, - DINA30, DINA38 - }; - unsigned char regs2[4] = { DINA20, DINA28, - DINA30, DINA38 - }; - inv_error_t result; - int_fast8_t kk; - int_fast8_t ii; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8); - - if (elements) { - fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT; - } else { - fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0; - } - - for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) { - fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0; - for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { - fifo_obj.decoded_accel[kk][ii] = 0; - } - } - - for (kk = 0; kk < 4; ++kk) { - if ((elements & 1) == 0) - regs[kk + 1] = DINAA0 + 3; - elements >>= 1; - } - - result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - for (kk = 0; kk < 4; ++kk) { - if ((elements & 1) == 0) - regs2[kk] = DINAA0 + 3; - elements >>= 1; - } - - result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return inv_set_footer(); -} - -inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy) -{ - INVENSENSE_FUNC_START; - int_fast8_t kk; - unsigned char regs[3] = { DINA28, DINA30, DINA38 }; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (accuracy) { - accuracy = INV_32_BIT; - } - - elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3); - accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS); - - fifo_obj.data_config[CONFIG_EIS] = elements | accuracy; - - for (kk = 0; kk < 3; ++kk) { - if ((elements & 1) == 0) - regs[kk] = DINAA0 + 7; - elements >>= 1; - } - - result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs); - - return inv_set_footer(); -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame. - * - * @param[out] data 3-element vector of accelerometer data in body frame. - * One gee = 2^16. - * @return 0 on success or an error code. - */ -inv_error_t inv_get_accel(long *data) -{ - int kk; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if ((!fifo_obj.data_config[CONFIG_ACCEL] && - (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) - || - (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && - !inv_accel_present())) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { - data[kk] = fifo_obj.decoded[REF_ACCEL + kk]; - } - - return INV_SUCCESS; -} - -/** - * @brief Returns 4-element quaternion vector derived from 6-axis or - * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is - * gyros, accel and compass. - * - * @param[out] data 4-element quaternion vector. One is scaled to 2^30. - * @return 0 on success or an error code. - */ -inv_error_t inv_get_quaternion(long *data) -{ - int kk; - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_QUAT]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = 0; kk < 4; ++kk) { - data[kk] = fifo_obj.decoded[REF_QUATERNION + kk]; - } - - return INV_SUCCESS; -} - -/** - * @brief Returns 4-element quaternion vector derived from 6 - * axis sensors (gyros and accels). - * @param[out] data - * 4-element quaternion vector. One is scaled to 2^30. - * @return 0 on success or an error code. - */ -inv_error_t inv_get_6axis_quaternion(long *data) -{ - int kk; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_QUAT]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = 0; kk < 4; ++kk) { - data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk]; - } - - return INV_SUCCESS; -} - -inv_error_t inv_get_relative_quaternion(long *data) -{ - if (data == NULL) - return INV_ERROR; - data[0] = inv_obj.relative_quat[0]; - data[1] = inv_obj.relative_quat[1]; - data[2] = inv_obj.relative_quat[2]; - data[3] = inv_obj.relative_quat[3]; - return INV_SUCCESS; -} - -/** - * @brief Returns 3-element vector of gyro data in body frame. - * @param[out] data - * 3-element vector of gyro data in body frame - * with gravity removed. One degree per second = 2^16. - * @return 0 on success or an error code. - */ -inv_error_t inv_get_gyro(long *data) -{ - int kk; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (fifo_obj.data_config[CONFIG_GYROS]) { - for (kk = 0; kk < 3; ++kk) { - data[kk] = fifo_obj.decoded[REF_GYROS + kk]; - } - return INV_SUCCESS; - } else { - return INV_ERROR_FEATURE_NOT_ENABLED; - } -} - -/** - * @brief Get the 3-element gravity vector from the FIFO expressed - * in coordinates relative to the body frame. - * @param data - * 3-element vector of gravity in body frame. - * @return 0 on success or an error code. - */ -inv_error_t inv_get_gravity(long *data) -{ - long quat[4]; - int ii; - inv_error_t result; - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_QUAT]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) { - fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY; - - // Compute it from Quaternion - result = inv_get_quaternion(quat); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - data[0] = - inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); - data[1] = - inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); - data[2] = - (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) - - 1073741824L; - - for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { - data[ii] >>= 14; - fifo_obj.gravity_cache[ii] = data[ii]; - } - } else { - data[0] = fifo_obj.gravity_cache[0]; - data[1] = fifo_obj.gravity_cache[1]; - data[2] = fifo_obj.gravity_cache[2]; - } - - return INV_SUCCESS; -} - -/** -* @brief Sets the filter coefficent used for computing the acceleration -* bias which is used to compute linear acceleration. -* @param[in] coef Fitler coefficient. 0. means no filter, a small number means -* a small cutoff frequency with an increasing number meaning -* an increasing cutoff frequency. -*/ -inv_error_t inv_set_linear_accel_filter_coef(float coef) -{ - fifo_obj.acc_filter_coef = coef; - return INV_SUCCESS; -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame - * with gravity removed. - * @param[out] data 3-element vector of accelerometer data in body frame - * with gravity removed. One g = 2^16. - * @return 0 on success or an error code. data unchanged on error. - */ -inv_error_t inv_get_linear_accel(long *data) -{ - int kk; - long grav[3]; - long la[3]; - inv_error_t result; - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - result = inv_get_gravity(grav); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_get_accel(la); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) { - fifo_obj.cache |= FIFO_CACHE_ACC_BIAS; - - for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { - long x; - x = la[kk] - grav[kk]; - fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef + - fifo_obj.acc_bias_filt[kk] * - (1.f - - fifo_obj.acc_filter_coef)); - data[kk] = x - fifo_obj.acc_bias_filt[kk]; - } - } else { - for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { - data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk]; - } - } - return INV_SUCCESS; -} - -/** - * @brief Returns 3-element vector of accelerometer data in world frame - * with gravity removed. - * @param[out] data 3-element vector of accelerometer data in world frame - * with gravity removed. One g = 2^16. - * @return 0 on success or an error code. - */ -inv_error_t inv_get_linear_accel_in_world(long *data) -{ - int kk; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) { - long wtemp[4], qi[4], wtemp2[4]; - wtemp[0] = 0; - inv_get_linear_accel(&wtemp[1]); - inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2); - inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi); - inv_q_mult(wtemp2, qi, wtemp); - for (kk = 0; kk < 3; ++kk) { - data[kk] = wtemp[kk + 1]; - } - return INV_SUCCESS; - } else { - return INV_ERROR_FEATURE_NOT_ENABLED; - } -} - -/** - * @brief Returns 4-element vector of control data. - * @param[out] data 4-element vector of control data. - * @return 0 for succes or an error code. - */ -inv_error_t inv_get_cntrl_data(long *data) -{ - int kk; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_CONTROL_DATA]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = 0; kk < 4; ++kk) { - data[kk] = fifo_obj.decoded[REF_CONTROL + kk]; - } - - return INV_SUCCESS; - -} - -/** - * @brief Returns 3-element vector of EIS shfit data - * @param[out] data 3-element vector of EIS shift data. - * @return 0 for succes or an error code. - */ -inv_error_t inv_get_eis(long *data) -{ - int kk; - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_EIS]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = 0; kk < 3; ++kk) { - data[kk] = fifo_obj.decoded[REF_EIS + kk]; - } - - return INV_SUCCESS; - -} - -/** - * @brief Returns 3-element vector of accelerometer data in body frame. - * @param[out] data 3-element vector of accelerometer data in body frame in g's. - * @return 0 for success or an error code. - */ -inv_error_t inv_get_accel_float(float *data) -{ - long lData[3]; - int kk; - int result; - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - result = inv_get_accel(lData); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { - data[kk] = lData[kk] / 65536.0f; - } - - return INV_SUCCESS; -} - -/** - * @brief Returns 4-element quaternion vector. - * @param[out] data 4-element quaternion vector. - * @return 0 on success, an error code otherwise. - */ -inv_error_t inv_get_quaternion_float(float *data) -{ - int kk; - - if (data == NULL) - return INV_ERROR_INVALID_PARAMETER; - - if (!fifo_obj.data_config[CONFIG_QUAT]) - return INV_ERROR_FEATURE_NOT_ENABLED; - - for (kk = 0; kk < 4; ++kk) { - data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f; - } - - return INV_SUCCESS; -} - -/** - * @brief Command the MPU to put data in the FIFO at a particular rate. - * - * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For - * example if the MPU is running at 200Hz the following values apply: - * - * <TABLE> - * <TR><TD>fifoRate</TD><TD>DMP Sample Rate</TD><TD>FIFO update frequency</TD></TR> - * <TR><TD>0</TD><TD>200Hz</TD><TD>200Hz</TD></TR> - * <TR><TD>1</TD><TD>200Hz</TD><TD>100Hz</TD></TR> - * <TR><TD>2</TD><TD>200Hz</TD><TD>50Hz</TD></TR> - * <TR><TD>4</TD><TD>200Hz</TD><TD>40Hz</TD></TR> - * <TR><TD>9</TD><TD>200Hz</TD><TD>20Hz</TD></TR> - * <TR><TD>19</TD><TD>200Hz</TD><TD>10Hz</TD></TR> - * </TABLE> - * - * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED) - * then inv_run_state_callbacks() will be called to allow features - * that depend upon fundamental constants to be updated. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * and inv_dmp_start() - * must <b>NOT</b> have been called. - * - * @param fifoRate Divider value - 1. Output rate is - * (DMP Sample Rate) / (fifoRate + 1). - * - * @return INV_SUCCESS if successful, ML error code on any failure. - */ -inv_error_t inv_set_fifo_rate(unsigned short fifoRate) -{ - INVENSENSE_FUNC_START; - unsigned char regs[2]; - unsigned char state; - inv_error_t result = INV_SUCCESS; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - state = inv_get_state(); - if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - fifo_obj.fifo_rate = fifoRate; - - if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { - - regs[0] = (unsigned char)((fifoRate >> 8) & 0xff); - regs[1] = (unsigned char)(fifoRate & 0xff); - - result = inv_set_mpu_memory(KEY_D_0_22, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - fifo_obj.sample_step_size_ms = - (unsigned short)(((long)fifoRate + 1) * - (inv_mpu_get_sampling_period_us - (mldl_cfg)) / 1000L); - - if (state == INV_STATE_DMP_STARTED) - result = inv_run_state_callbacks(state); - } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) { - struct ext_slave_config config; - long data; - config.key = MPU_SLAVE_CONFIG_ODR_RESUME; - config.len = sizeof(long); - config.apply = (state == INV_STATE_DMP_STARTED); - config.data = &data; - data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1); - - /* Ask for the same frequency */ - result = inv_mpu_config_accel(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_mpu_get_accel_config(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), &config); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - if(FIFO_DEBUG) - MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000); - /* Record the actual frequency granted odr is in mHz */ - fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data); - } - return result; -} - -/** - * @brief Retrieve the current FIFO update divider - 1. - * See inv_set_fifo_rate() for how this value is used. - * - * The fifo rate when there is no fifo is the equivilent divider when - * derived from the value set by SetSampleSteSizeMs() - * - * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error. - */ -unsigned short inv_get_fifo_rate(void) -{ - return fifo_obj.fifo_rate; -} - -/** - * @brief Returns the step size for quaternion type data. - * - * Typically the data rate for each FIFO packet. When the gryos are sleeping - * this value will return the last value set by SetSampleStepSizeMs() - * - * @return step size for quaternion type data - */ -int_fast16_t inv_get_sample_step_size_ms(void) -{ - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) - return (fifo_obj.fifo_rate + 1) * - (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000); - else - return fifo_obj.sample_step_size_ms; -} - -/** - * @brief Returns the step size for quaternion type data. - * - * Typically the data rate for each FIFO packet. When the gryos are sleeping - * this value will return the last value set by SetSampleStepSizeMs() - * - * @return step size for quaternion type data - */ -int_fast16_t inv_get_sample_frequency(void) -{ - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) - return (inv_mpu_get_sampling_rate_hz(mldl_cfg) / - (fifo_obj.fifo_rate + 1)); - else - return (1000 / fifo_obj.sample_step_size_ms); -} - -/** - * @brief The gyro data magnitude squared : - * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. - * @return the computed magnitude squared output of the gyroscope. - */ -unsigned long inv_get_gyro_sum_of_sqr(void) -{ - unsigned long gmag = 0; - long temp; - int kk; - - for (kk = 0; kk < 3; ++kk) { - temp = fifo_obj.decoded[REF_GYROS + kk] >> - (16 - (GYRO_MAG_SQR_SHIFT / 2)); - gmag += temp * temp; - } - - return gmag; -} - -/** - * @brief The gyro data magnitude squared: - * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT. - * @return the computed magnitude squared output of the accelerometer. - */ -unsigned long inv_accel_sum_of_sqr(void) -{ - unsigned long amag = 0; - long temp; - int kk; - long accel[3]; - inv_error_t result; - - result = inv_get_accel(accel); - if (INV_SUCCESS != result) { - return 0; - } - - for (kk = 0; kk < 3; ++kk) { - temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2)); - amag += temp * temp; - } - return amag; -} - -/** - * @internal - */ -void inv_override_quaternion(float *q) -{ - int kk; - for (kk = 0; kk < 4; ++kk) { - fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30)); - } -} - -/** - * @internal - * @brief This registers a function to be called for each set of - * gyro/quaternion/rotation matrix/etc output. - * @param[in] func The callback function to register - * @param[in] priority The unique priority number of the callback. Lower numbers - * are called first. - * @return error code. - */ -inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - int kk, nn; - - result = inv_lock_mutex(fifo_rate_obj.mutex); - if (INV_SUCCESS != result) { - return result; - } - // Make sure we haven't registered this function already - // Or used the same priority - for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { - if ((fifo_rate_obj.fifo_process_cb[kk] == func) || - (fifo_rate_obj.priority[kk] == priority)) { - inv_unlock_mutex(fifo_rate_obj.mutex); - return INV_ERROR_INVALID_PARAMETER; - } - } - - // Make sure we have not filled up our number of allowable callbacks - if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) { - kk = 0; - if (fifo_rate_obj.num_cb != 0) { - // set kk to be where this new callback goes in the array - while ((kk < fifo_rate_obj.num_cb) && - (fifo_rate_obj.priority[kk] < priority)) { - kk++; - } - if (kk != fifo_rate_obj.num_cb) { - // We need to move the others - for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) { - fifo_rate_obj.fifo_process_cb[nn] = - fifo_rate_obj.fifo_process_cb[nn - 1]; - fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1]; - } - } - } - // Add new callback - fifo_rate_obj.fifo_process_cb[kk] = func; - fifo_rate_obj.priority[kk] = priority; - fifo_rate_obj.num_cb++; - } else { - result = INV_ERROR_MEMORY_EXAUSTED; - } - - inv_unlock_mutex(fifo_rate_obj.mutex); - return result; -} - -/** - * @internal - * @brief This unregisters a function to be called for each set of - * gyro/quaternion/rotation matrix/etc output. - * @return error code. - */ -inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func) -{ - INVENSENSE_FUNC_START; - int kk, jj; - inv_error_t result; - - result = inv_lock_mutex(fifo_rate_obj.mutex); - if (INV_SUCCESS != result) { - return result; - } - // Make sure we haven't registered this function already - result = INV_ERROR_INVALID_PARAMETER; - for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { - if (fifo_rate_obj.fifo_process_cb[kk] == func) { - for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) { - fifo_rate_obj.fifo_process_cb[jj - 1] = - fifo_rate_obj.fifo_process_cb[jj]; - fifo_rate_obj.priority[jj - 1] = - fifo_rate_obj.priority[jj]; - } - fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL; - fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0; - fifo_rate_obj.num_cb--; - result = INV_SUCCESS; - break; - } - } - - inv_unlock_mutex(fifo_rate_obj.mutex); - return result; - -} -#ifdef UMPL -bool bFIFIDataAvailable = FALSE; -bool isUmplDataInFIFO(void) -{ - return bFIFIDataAvailable; -} -void setUmplDataInFIFOFlag(bool flag) -{ - bFIFIDataAvailable = flag; -} -#endif -inv_error_t inv_run_fifo_rate_processes(void) -{ - int kk; - inv_error_t result, result2; - - result = inv_lock_mutex(fifo_rate_obj.mutex); - if (INV_SUCCESS != result) { - MPL_LOGE("MLOsLockMutex returned %d\n", result); - return result; - } - // User callbacks take priority over the fifo_process_cb callback - if (fifo_obj.fifo_process_cb) - fifo_obj.fifo_process_cb(); - - for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { - if (fifo_rate_obj.fifo_process_cb[kk]) { - result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj); - if (result == INV_SUCCESS) -#ifdef UMPL - setUmplDataInFIFOFlag(TRUE); -#endif - result = result2; - } - } - - inv_unlock_mutex(fifo_rate_obj.mutex); - return result; -} - -/*********************/ - /** \}*//* defgroup */ -/*********************/ diff --git a/60xx/mlsdk/mllite/mlFIFO.h b/60xx/mlsdk/mllite/mlFIFO.h deleted file mode 100644 index 2114eb3..0000000 --- a/60xx/mlsdk/mllite/mlFIFO.h +++ /dev/null @@ -1,150 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef INVENSENSE_INV_FIFO_H__ -#define INVENSENSE_INV_FIFO_H__ - -#include "mltypes.h" -#include "mlinclude.h" -#include "ml.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlFIFO_legacy.h" -#endif - -#ifdef __cplusplus -extern "C" { -#endif - - /**************************************************************************/ - /* Elements */ - /**************************************************************************/ - -#define INV_ELEMENT_1 (0x0001) -#define INV_ELEMENT_2 (0x0002) -#define INV_ELEMENT_3 (0x0004) -#define INV_ELEMENT_4 (0x0008) -#define INV_ELEMENT_5 (0x0010) -#define INV_ELEMENT_6 (0x0020) -#define INV_ELEMENT_7 (0x0040) -#define INV_ELEMENT_8 (0x0080) - -#define INV_ALL (0xFFFF) -#define INV_ELEMENT_MASK (0x00FF) - - /**************************************************************************/ - /* Accuracy */ - /**************************************************************************/ - -#define INV_16_BIT (0x0100) -#define INV_32_BIT (0x0200) -#define INV_ACCURACY_MASK (0x0300) - - /**************************************************************************/ - /* Accuracy */ - /**************************************************************************/ - -#define INV_GYRO_FROM_RAW (0x00) -#define INV_GYRO_FROM_QUATERNION (0x01) - - /**************************************************************************/ - /* High Rate Proceses */ - /**************************************************************************/ - -#define MAX_HIGH_RATE_PROCESSES 16 - - /**************************************************************************/ - /* Prototypes */ - /**************************************************************************/ - - inv_error_t inv_set_fifo_rate(unsigned short fifoRate); - unsigned short inv_get_fifo_rate(void); - int_fast16_t inv_get_sample_step_size_ms(void); - int_fast16_t inv_get_sample_frequency(void); - long inv_decode_temperature(short tempReg); - - // Register callbacks after a packet of FIFO data is processed - inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority); - inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func); - inv_error_t inv_run_fifo_rate_processes(void); - - // Setup FIFO for various output - inv_error_t inv_send_quaternion(uint_fast16_t accuracy); - inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy); - inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy); - inv_error_t inv_send_linear_accel(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_cntrl_data(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_sensor_data(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_external_sensor_data(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_gravity(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_packet_number(uint_fast16_t accuracy); - inv_error_t inv_send_quantized_accel(uint_fast16_t elements, - uint_fast16_t accuracy); - inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - inv_error_t inv_get_accel(long *data); - inv_error_t inv_get_quaternion(long *data); - inv_error_t inv_get_6axis_quaternion(long *data); - inv_error_t inv_get_relative_quaternion(long *data); - inv_error_t inv_get_gyro(long *data); - inv_error_t inv_set_linear_accel_filter_coef(float coef); - inv_error_t inv_get_linear_accel(long *data); - inv_error_t inv_get_linear_accel_in_world(long *data); - inv_error_t inv_get_gyro_and_accel_sensor(long *data); - inv_error_t inv_get_gyro_sensor(long *data); - inv_error_t inv_get_cntrl_data(long *data); - inv_error_t inv_get_temperature(long *data); - inv_error_t inv_get_gravity(long *data); - inv_error_t inv_get_unquantized_accel(long *data); - inv_error_t inv_get_quantized_accel(long *data); - inv_error_t inv_get_external_sensor_data(long *data, int size); - inv_error_t inv_get_eis(long *data); - - // Get Floating Point data from FIFO - inv_error_t inv_get_accel_float(float *data); - inv_error_t inv_get_quaternion_float(float *data); - - inv_error_t inv_process_fifo_packet(const unsigned char *dmpData); - inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, - int_fast8_t * processed); - - inv_error_t inv_set_fifo_processed_callback(void (*func) (void)); - - inv_error_t inv_init_fifo_param(void); - inv_error_t inv_close_fifo(void); - inv_error_t inv_set_gyro_data_source(uint_fast8_t source); - inv_error_t inv_decode_quantized_accel(void); - unsigned long inv_get_gyro_sum_of_sqr(void); - unsigned long inv_accel_sum_of_sqr(void); - void inv_override_quaternion(float *q); -#ifdef UMPL - bool isUmplDataInFIFO(void); - void setUmplDataInFIFOFlag(bool flag); -#endif - uint_fast16_t inv_get_fifo_packet_size(void); -#ifdef __cplusplus -} -#endif -#endif // INVENSENSE_INV_FIFO_H__ diff --git a/60xx/mlsdk/mllite/mlFIFOHW.c b/60xx/mlsdk/mllite/mlFIFOHW.c deleted file mode 100644 index 7a77e66..0000000 --- a/60xx/mlsdk/mllite/mlFIFOHW.c +++ /dev/null @@ -1,328 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $ - * - *******************************************************************************/ - -/** - * @defgroup MLFIFO_HW - * @brief Motion Library - FIFO HW Driver. - * Provides facilities to interact with the FIFO. - * - * @{ - * @file mlFIFOHW.c - * @brief The Motion Library Fifo Hardware Layer. - * - */ - -#include <string.h> - -#include "mpu.h" -#if defined CONFIG_MPU_SENSORS_MPU6050A2 -# include "mpu6050a2.h" -#elif defined CONFIG_MPU_SENSORS_MPU6050B1 -# include "mpu6050b1.h" -#elif defined CONFIG_MPU_SENSORS_MPU3050 -# include "mpu3050.h" -#else -#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx -#endif -#include "mlFIFOHW.h" -#include "ml.h" -#include "mldl.h" -#include "mldl_cfg.h" - -#include "mlsl.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-fifo" - -/* - Defines -*/ - -#define _fifoDebug(x) //{x} - -/* - Typedefs -*/ - -struct fifo_hw_obj { - short fifoCount; - inv_error_t fifoError; - unsigned char fifoOverflow; - unsigned char fifoResetOnOverflow; -}; - -/* - Global variables -*/ -const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A }; - -/* - Static variables -*/ -static struct fifo_hw_obj fifo_objHW; - -/* - Definitions -*/ - -/** - * @brief Initializes the internal FIFO data structure. - */ -void inv_init_fifo_hardare(void) -{ - memset(&fifo_objHW, 0, sizeof(fifo_objHW)); - fifo_objHW.fifoResetOnOverflow = TRUE; -} - -/** - * @internal - * @brief used to get the FIFO data. - * @param length - * Number of bytes to read from the FIFO. - * @param buffer - * the bytes of FIFO data. - * Note that this buffer <b>must</b> be large enough - * to store and additional trailing FIFO footer when - * expected. The callers must make sure enough space - * is allocated. - * @return number of valid bytes of data. -**/ -uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - uint_fast16_t inFifo; - uint_fast16_t toRead; - int_fast8_t kk; - - toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount; - /*---- make sure length is correct ----*/ - if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) { - fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER; - return 0; - } - - result = inv_get_fifo_length(&inFifo); - if (INV_SUCCESS != result) { - fifo_objHW.fifoError = result; - return 0; - } - // fifo_objHW.fifoCount is the footer size left in the buffer, or - // 0 if this is the first time reading the fifo since it was reset - if (inFifo < length + fifo_objHW.fifoCount) { - fifo_objHW.fifoError = INV_SUCCESS; - return 0; - } - // if a trailing fifo count is expected - start storing data 2 bytes before - result = - inv_read_fifo(fifo_objHW.fifoCount > - 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead); - if (INV_SUCCESS != result) { - fifo_objHW.fifoError = result; - return 0; - } - // Make sure the fifo didn't overflow before or during the read - result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), - MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow); - if (INV_SUCCESS != result) { - fifo_objHW.fifoError = result; - return 0; - } - - if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) { - MPL_LOGV("Resetting Fifo : Overflow\n"); - inv_reset_fifo(); - fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW; - return 0; - } - - /* Check the Footer value to give us a chance at making sure data - * didn't get corrupted */ - for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) { - if (buffer[kk] != gFifoFooter[kk]) { - MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n", - buffer[0], buffer[1]); - _fifoDebug(char out[200]; - MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount); - sprintf(out, "0x"); - for (kk = 0; kk < (int)toRead; kk++) { - sprintf(out, "%s%02X", out, buffer[kk]);} - MPL_LOGW("%s\n", out);) - inv_reset_fifo(); - fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER; - return 0; - } - } - - if (fifo_objHW.fifoCount == 0) { - fifo_objHW.fifoCount = FIFO_FOOTER_SIZE; - } - - return length - FIFO_FOOTER_SIZE; -} - -/** - * @brief Used to query the status of the FIFO. - * @return INV_SUCCESS if the fifo is OK. An error code otherwise. -**/ -inv_error_t inv_get_fifo_status(void) -{ - inv_error_t fifoError = fifo_objHW.fifoError; - fifo_objHW.fifoError = 0; - return fifoError; -} - -/** - * @internal - * @brief Get the length from the fifo - * - * @param[out] len amount of data currently stored in the fifo. - * - * @return INV_SUCCESS or non-zero error code. -**/ -inv_error_t inv_get_fifo_length(uint_fast16_t * len) -{ - INVENSENSE_FUNC_START; - unsigned char fifoBuf[2]; - inv_error_t result; - - if (NULL == len) - return INV_ERROR_INVALID_PARAMETER; - - /*---- read the 2 'count' registers and - burst read the data from the FIFO ----*/ - result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), - MPUREG_FIFO_COUNTH, 2, fifoBuf); - if (INV_SUCCESS != result) { - MPL_LOGE("ReadBurst failed %d\n", result); - inv_reset_fifo(); - fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT; - *len = 0; - return result; - } - - *len = (uint_fast16_t) (fifoBuf[0] << 8); - *len += (uint_fast16_t) (fifoBuf[1]); - return result; -} - -/** - * @brief inv_get_fifo_count is used to get the number of bytes left in the FIFO. - * This function returns the stored value and does not access the hardware. - * See inv_get_fifo_length(). - * @return the number of bytes left in the FIFO -**/ -short inv_get_fifo_count(void) -{ - return fifo_objHW.fifoCount; -} - -/** - * @internal - * @brief Read data from the fifo - * - * @param[out] data Location to store the date read from the fifo - * @param[in] len Amount of data to read out of the fifo - * - * @return INV_SUCCESS or non-zero error code -**/ -inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - result = inv_serial_read_fifo(inv_get_serial_handle(), - inv_get_mpu_slave_addr(), - (unsigned short)len, data); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_serial_readBurst failed %d\n", result); - inv_reset_fifo(); - fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA; - return result; - } - return result; -} - -/** - * @brief Clears the FIFO status and its content. - * @note Halt the DMP writing into the FIFO for the time - * needed to reset the FIFO. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_reset_fifo(void) -{ - INVENSENSE_FUNC_START; - int len = FIFO_HW_SIZE; - unsigned char fifoBuf[2]; - unsigned char tries = 0; - unsigned char userCtrlReg; - inv_error_t result; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - fifo_objHW.fifoCount = 0; - if (mldl_cfg->gyro_is_suspended) - return INV_SUCCESS; - - result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), - MPUREG_USER_CTRL, 1, &userCtrlReg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - while (len != 0 && tries < 6) { - result = - inv_serial_single_write(inv_get_serial_handle(), - inv_get_mpu_slave_addr(), MPUREG_USER_CTRL, - ((userCtrlReg & (~BIT_FIFO_EN)) | - BIT_FIFO_RST)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = - inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), - MPUREG_FIFO_COUNTH, 2, fifoBuf); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1]; - tries++; - } - - result = - inv_serial_single_write(inv_get_serial_handle(), - inv_get_mpu_slave_addr(), MPUREG_USER_CTRL, - userCtrlReg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return INV_SUCCESS; -} - -/** - * @} -**/ diff --git a/60xx/mlsdk/mllite/mlFIFOHW.h b/60xx/mlsdk/mllite/mlFIFOHW.h deleted file mode 100644 index 6f70185..0000000 --- a/60xx/mlsdk/mllite/mlFIFOHW.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef INVENSENSE_INV_FIFO_HW_H__ -#define INVENSENSE_INV_FIFO_HW_H__ - -#include "mpu.h" -#include "mltypes.h" -#include "mlinclude.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlFIFOHW_legacy.h" -#endif - -#ifdef __cplusplus -extern "C" { -#endif - - // This is the maximum amount of FIFO data we would read in one packet -#define MAX_FIFO_LENGTH (256) - // This is the hardware size of the FIFO -#define FIFO_FOOTER_SIZE (2) - - uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer); - inv_error_t inv_get_fifo_status(void); - inv_error_t inv_get_fifo_length(uint_fast16_t * len); - short inv_get_fifo_count(void); - inv_error_t inv_reset_fifo(void); - void inv_init_fifo_hardare(); - inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len); - -#ifdef __cplusplus -} -#endif -#endif // INVENSENSE_INV_FIFO_HW_H__ diff --git a/60xx/mlsdk/mllite/mlMathFunc.c b/60xx/mlsdk/mllite/mlMathFunc.c deleted file mode 100644 index 0d8e7ec..0000000 --- a/60xx/mlsdk/mllite/mlMathFunc.c +++ /dev/null @@ -1,377 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#include "mlmath.h" -#include "mlMathFunc.h" -#include "mlinclude.h" - -/** - * Performs one iteration of the filter, generating a new y(0) - * 1 / N / N \\ - * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length - * a(0) \k=0 \ k=1 // - * - * The filters A and B should be (sizeof(long) * state->length). - * The state variables x and y should be (sizeof(long) * (state->length - 1)) - * - * The state variables x and y should be initialized prior to the first call - * - * @param state Contains the length of the filter, filter coefficients and - * filter state variables x and y. - * @param x New input into the filter. - */ -void inv_filter_long(struct filter_long *state, long x) -{ - const long *b = state->b; - const long *a = state->a; - long length = state->length; - long long tmp; - int ii; - - /* filter */ - tmp = (long long)x *(b[0]); - for (ii = 0; ii < length - 1; ii++) { - tmp += ((long long)state->x[ii] * (long long)(b[ii + 1])); - } - for (ii = 0; ii < length - 1; ii++) { - tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1])); - } - tmp /= (long long)a[0]; - - /* Delay */ - for (ii = length - 2; ii > 0; ii--) { - state->y[ii] = state->y[ii - 1]; - state->x[ii] = state->x[ii - 1]; - } - /* New values */ - state->y[0] = (long)tmp; - state->x[0] = x; -} - -/** Performs a multiply and shift by 29. These are good functions to write in assembly on - * with devices with small memory where you want to get rid of the long long which some - * assemblers don't handle well - * @param[in] a - * @param[in] b - * @return ((long long)a*b)>>29 -*/ -long inv_q29_mult(long a, long b) -{ - long long temp; - long result; - temp = (long long)a *b; - result = (long)(temp >> 29); - return result; -} - -/** Performs a multiply and shift by 30. These are good functions to write in assembly on - * with devices with small memory where you want to get rid of the long long which some - * assemblers don't handle well - * @param[in] a - * @param[in] b - * @return ((long long)a*b)>>30 -*/ -long inv_q30_mult(long a, long b) -{ - long long temp; - long result; - temp = (long long)a *b; - result = (long)(temp >> 30); - return result; -} - -void inv_q_mult(const long *q1, const long *q2, long *qProd) -{ - INVENSENSE_FUNC_START; - qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] - - (long long)q1[2] * q2[2] - - (long long)q1[3] * q2[3]) >> 30); - qProd[1] = - (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] + - (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30); - qProd[2] = - (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] + - (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30); - qProd[3] = - (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] - - (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30); -} - -void inv_q_add(long *q1, long *q2, long *qSum) -{ - INVENSENSE_FUNC_START; - qSum[0] = q1[0] + q2[0]; - qSum[1] = q1[1] + q2[1]; - qSum[2] = q1[2] + q2[2]; - qSum[3] = q1[3] + q2[3]; -} - -void inv_q_normalize(long *q) -{ - INVENSENSE_FUNC_START; - double normSF = 0; - int i; - for (i = 0; i < 4; i++) { - normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L; - } - if (normSF > 0) { - normSF = 1 / sqrt(normSF); - for (i = 0; i < 4; i++) { - q[i] = (int)((double)q[i] * normSF); - } - } else { - q[0] = 1073741824L; - q[1] = 0; - q[2] = 0; - q[3] = 0; - } -} - -void inv_q_invert(const long *q, long *qInverted) -{ - INVENSENSE_FUNC_START; - qInverted[0] = q[0]; - qInverted[1] = -q[1]; - qInverted[2] = -q[2]; - qInverted[3] = -q[3]; -} - -void inv_q_multf(const float *q1, const float *q2, float *qProd) -{ - INVENSENSE_FUNC_START; - qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); - qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); - qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); - qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); -} - -void inv_q_addf(float *q1, float *q2, float *qSum) -{ - INVENSENSE_FUNC_START; - qSum[0] = q1[0] + q2[0]; - qSum[1] = q1[1] + q2[1]; - qSum[2] = q1[2] + q2[2]; - qSum[3] = q1[3] + q2[3]; -} - -void inv_q_normalizef(float *q) -{ - INVENSENSE_FUNC_START; - float normSF = 0; - float xHalf = 0; - normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - if (normSF < 2) { - xHalf = 0.5f * normSF; - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - normSF = normSF * (1.5f - xHalf * normSF * normSF); - q[0] *= normSF; - q[1] *= normSF; - q[2] *= normSF; - q[3] *= normSF; - } else { - q[0] = 1.0; - q[1] = 0.0; - q[2] = 0.0; - q[3] = 0.0; - } - normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); -} - -/** Performs a length 4 vector normalization with a square root. -* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero. -*/ -void inv_q_norm4(float *q) -{ - float mag; - mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - if (mag) { - q[0] /= mag; - q[1] /= mag; - q[2] /= mag; - q[3] /= mag; - } else { - q[0] = 1.f; - q[1] = 0.f; - q[2] = 0.f; - q[3] = 0.f; - } -} - -void inv_q_invertf(const float *q, float *qInverted) -{ - INVENSENSE_FUNC_START; - qInverted[0] = q[0]; - qInverted[1] = -q[1]; - qInverted[2] = -q[2]; - qInverted[3] = -q[3]; -} - -/** - * Converts a quaternion to a rotation matrix. - * @param[in] quat 4-element quaternion in fixed point. One is 2^30. - * @param[out] rot Rotation matrix in fixed point. One is 2^30. The - * First 3 elements of the rotation matrix, represent - * the first row of the matrix. Rotation matrix multiplied - * by a 3 element column vector transform a vector from Body - * to World. - */ -void inv_quaternion_to_rotation(const long *quat, long *rot) -{ - rot[0] = - inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], - quat[0]) - 1073741824L; - rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); - rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); - rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); - rot[4] = - inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], - quat[0]) - 1073741824L; - rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); - rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); - rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); - rot[8] = - inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], - quat[0]) - 1073741824L; -} - -/** Converts a 32-bit long to a big endian byte stream */ -unsigned char *inv_int32_to_big8(long x, unsigned char *big8) -{ - big8[0] = (unsigned char)((x >> 24) & 0xff); - big8[1] = (unsigned char)((x >> 16) & 0xff); - big8[2] = (unsigned char)((x >> 8) & 0xff); - big8[3] = (unsigned char)(x & 0xff); - return big8; -} - -/** Converts a big endian byte stream into a 32-bit long */ -long inv_big8_to_int32(const unsigned char *big8) -{ - long x; - x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) | - ((long)big8[3]); - return x; -} - -/** Converts a 16-bit short to a big endian byte stream */ -unsigned char *inv_int16_to_big8(short x, unsigned char *big8) -{ - big8[0] = (unsigned char)((x >> 8) & 0xff); - big8[1] = (unsigned char)(x & 0xff); - return big8; -} - -void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) -{ - int k, l, i, j; - for (i = 0, k = 0; i < *n; i++, k++) { - for (j = 0, l = 0; j < *n; j++, l++) { - if (i == x) - i++; - if (j == y) - j++; - *(b + 10 * k + l) = *(a + 10 * i + j); - } - } - *n = *n - 1; -} - -void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) -{ - int k, l, i, j; - for (i = 0, k = 0; i < *n; i++, k++) { - for (j = 0, l = 0; j < *n; j++, l++) { - if (i == x) - i++; - if (j == y) - j++; - *(b + 10 * k + l) = *(a + 10 * i + j); - } - } - *n = *n - 1; -} - -float inv_matrix_det(float *p, int *n) -{ - float d[10][10], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 11) - *(p + 1) ** (p + 10)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_inc(p, &d[0][0], n, i, j); - sum = - sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0], - n); - } - - return (sum); -} - -double inv_matrix_detd(double *p, int *n) -{ - double d[10][10], sum = 0; - int i, j, m; - m = *n; - if (*n == 2) - return (*p ** (p + 11) - *(p + 1) ** (p + 10)); - for (i = 0, j = 0; j < m; j++) { - *n = m; - inv_matrix_det_incd(p, &d[0][0], n, i, j); - sum = - sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0], - n); - } - - return (sum); -} - -/** Wraps angle from (-M_PI,M_PI] - * @param[in] ang Angle in radians to wrap - * @return Wrapped angle from (-M_PI,M_PI] - */ -float inv_wrap_angle(float ang) -{ - if (ang > M_PI) - return ang - 2 * (float)M_PI; - else if (ang <= -(float)M_PI) - return ang + 2 * (float)M_PI; - else - return ang; -} - -/** Finds the minimum angle difference ang1-ang2 such that difference - * is between [-M_PI,M_PI] - * @param[in] ang1 - * @param[in] ang2 - * @return angle difference ang1-ang2 - */ -float inv_angle_diff(float ang1, float ang2) -{ - float d; - ang1 = inv_wrap_angle(ang1); - ang2 = inv_wrap_angle(ang2); - d = ang1 - ang2; - if (d > M_PI) - d -= 2 * (float)M_PI; - else if (d < -(float)M_PI) - d += 2 * (float)M_PI; - return d; -} diff --git a/60xx/mlsdk/mllite/mlMathFunc.h b/60xx/mlsdk/mllite/mlMathFunc.h deleted file mode 100644 index 70fa9f4..0000000 --- a/60xx/mlsdk/mllite/mlMathFunc.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef INVENSENSE_INV_MATH_FUNC_H__ -#define INVENSENSE_INV_MATH_FUNC_H__ -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlMathFunc_legacy.h" -#endif - -#define NUM_ROTATION_MATRIX_ELEMENTS (9) -#define ROT_MATRIX_SCALE_LONG (1073741824) -#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) -#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ - ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) -#define SIGNM(k)((int)(k)&1?-1:1) - -#ifdef __cplusplus -extern "C" { -#endif - struct filter_long { - int length; - const long *b; - const long *a; - long *x; - long *y; - }; - - void inv_filter_long(struct filter_long *state, long x); - long inv_q29_mult(long a, long b); - long inv_q30_mult(long a, long b); - void inv_q_mult(const long *q1, const long *q2, long *qProd); - void inv_q_add(long *q1, long *q2, long *qSum); - void inv_q_normalize(long *q); - void inv_q_invert(const long *q, long *qInverted); - void inv_q_multf(const float *q1, const float *q2, float *qProd); - void inv_q_addf(float *q1, float *q2, float *qSum); - void inv_q_normalizef(float *q); - void inv_q_norm4(float *q); - void inv_q_invertf(const float *q, float *qInverted); - void inv_quaternion_to_rotation(const long *quat, long *rot); - unsigned char *inv_int32_to_big8(long x, unsigned char *big8); - long inv_big8_to_int32(const unsigned char *big8); - unsigned char *inv_int16_to_big8(short x, unsigned char *big8); - float inv_matrix_det(float *p, int *n); - void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); - double inv_matrix_detd(double *p, int *n); - void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); - float inv_wrap_angle(float ang); - float inv_angle_diff(float ang1, float ang2); - -#ifdef __cplusplus -} -#endif -#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/60xx/mlsdk/mllite/mlSetGyroBias.c b/60xx/mlsdk/mllite/mlSetGyroBias.c deleted file mode 100644 index bd14d2e..0000000 --- a/60xx/mlsdk/mllite/mlSetGyroBias.c +++ /dev/null @@ -1,198 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id:$ - * - *****************************************************************************/ - -#include "mlSetGyroBias.h" -#include "mlFIFO.h" -#include "ml.h" -#include <string.h> -#include "mldl.h" -#include "mlMathFunc.h" - -typedef struct { - int needToSetBias; - short currentBias[3]; - int mode; - int motion; -} tSGB; - -tSGB sgb; - -/** Records a motion event that may cause a callback when the priority for this - * feature is met. - */ -void inv_set_motion_state(int motion) -{ - sgb.motion = motion; -} - -/** Converts from internal DMP gyro bias registers to external hardware gyro bias by -* applying scaling and transformation. -*/ -void inv_convert_bias(const unsigned char *regs, short *bias) -{ - long biasTmp2[3], biasTmp[3], biasPrev[3]; - int i; - int sf; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (mldl_cfg->gyro_sens_trim != 0) { - sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; - } else { - sf = 2000; - } - for (i = 0; i < 3; i++) { - biasTmp2[i] = inv_big8_to_int32(®s[i * 4]); - } - // Rotate bias vector by the transpose of the orientation matrix - for (i = 0; i < 3; ++i) { - biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.gyro_orient[i]) + - inv_q30_mult(biasTmp2[1], inv_obj.gyro_orient[i + 3]) + - inv_q30_mult(biasTmp2[2], inv_obj.gyro_orient[i + 6]); - } - - for (i = 0; i < GYRO_NUM_AXES; i++) { - biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf); - biasPrev[i] = (long)mldl_cfg->offset[i]; - if (biasPrev[i] > 32767) - biasPrev[i] -= 65536L; - } - - for (i = 0; i < GYRO_NUM_AXES; i++) { - bias[i] = (short)(biasPrev[i] - biasTmp[i]); - } -} - -/** Records hardware biases in format as used by hardware gyro registers. -* Note, the hardware will add this value to the measured gyro data. -*/ -inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode) -{ - if (sgb.currentBias[0] != bias[0]) - sgb.needToSetBias = 1; - if (sgb.currentBias[1] != bias[1]) - sgb.needToSetBias = 1; - if (sgb.currentBias[2] != bias[2]) - sgb.needToSetBias = 1; - if (sgb.needToSetBias) { - memcpy(sgb.currentBias, bias, sizeof(sgb.currentBias)); - sgb.mode = mode; - } - return INV_SUCCESS; -} - -/** Records gyro biases -* @param[in] bias Bias where 1dps is 2^16. In chip frame. -*/ -inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode) -{ - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - int sf, i; - long biasTmp; - short offset[3]; - inv_error_t result; - - if (mldl_cfg->gyro_sens_trim != 0) { - sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; - } else { - sf = 2000; - } - - for (i = 0; i < GYRO_NUM_AXES; i++) { - biasTmp = -bias[i] / sf; - if (biasTmp < 0) - biasTmp += 65536L; - offset[i] = (short)biasTmp; - } - result = inv_set_gyro_bias_in_hw_unit(offset, mode); - return result; -} - -inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode) -{ - long biasL[3]; - inv_error_t result; - - biasL[0] = (long)(bias[0] * (1L << 16)); - biasL[1] = (long)(bias[1] * (1L << 16)); - biasL[2] = (long)(bias[2] * (1L << 16)); - result = inv_set_gyro_bias_in_dps(biasL, mode); - return result; -} - -inv_error_t MLSetGyroBiasCB(struct inv_obj_t * inv_obj) -{ - inv_error_t result = INV_SUCCESS; - if (sgb.needToSetBias) { - result = inv_set_offset(sgb.currentBias); - sgb.needToSetBias = 0; - } - - // Check if motion state has changed - if (sgb.motion == INV_MOTION) { - // We are moving - if (inv_obj->motion_state == INV_NO_MOTION) { - //Trigger motion callback - inv_obj->motion_state = INV_MOTION; - inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; - if (inv_params_obj.motion_cb_func) { - inv_params_obj.motion_cb_func(INV_MOTION); - } - } - } else if (sgb.motion == INV_NO_MOTION){ - // We are not moving - if (inv_obj->motion_state == INV_MOTION) { - //Trigger no motion callback - inv_obj->motion_state = INV_NO_MOTION; - inv_obj->got_no_motion_bias = TRUE; - inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_NO_MOTION; - if (inv_params_obj.motion_cb_func) { - inv_params_obj.motion_cb_func(INV_NO_MOTION); - } - } - } - - return result; -} - -inv_error_t inv_enable_set_bias(void) -{ - inv_error_t result; - memset(&sgb, 0, sizeof(sgb)); - - sgb.motion = inv_obj.motion_state; - - result = - inv_register_fifo_rate_process(MLSetGyroBiasCB, - INV_PRIORITY_SET_GYRO_BIASES); - if (result == INV_ERROR_INVALID_PARAMETER) - result = INV_SUCCESS; /* We already registered this */ - return result; -} - -inv_error_t inv_disable_set_bias(void) -{ - inv_error_t result; - result = inv_unregister_fifo_rate_process(MLSetGyroBiasCB); - return INV_SUCCESS; // FIXME need to disable -} diff --git a/60xx/mlsdk/mllite/mlSetGyroBias.h b/60xx/mlsdk/mllite/mlSetGyroBias.h deleted file mode 100644 index e56f18b..0000000 --- a/60xx/mlsdk/mllite/mlSetGyroBias.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef INV_SET_GYRO_BIAS__H__ -#define INV_SET_GYRO_BIAS__H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#define INV_SGB_NO_MOTION 4 -#define INV_SGB_FAST_NO_MOTION 5 -#define INV_SGB_TEMP_COMP 6 - - inv_error_t inv_enable_set_bias(void); - inv_error_t inv_disable_set_bias(void); - inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode); - inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode); - inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode); - void inv_convert_bias(const unsigned char *regs, short *bias); - void inv_set_motion_state(int motion); - -#ifdef __cplusplus -} -#endif -#endif // INV_SET_GYRO_BIAS__H__ diff --git a/60xx/mlsdk/mllite/ml_mputest.c b/60xx/mlsdk/mllite/ml_mputest.c deleted file mode 100644 index d7fc608..0000000 --- a/60xx/mlsdk/mllite/ml_mputest.c +++ /dev/null @@ -1,184 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: ml_mputest.c 5641 2011-06-14 02:10:02Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup MPU_SELF_TEST - * @brief C wrapper to integrate the MPU Self Test wrapper in MPL. - * Provides ML name compliant naming and an additional API that - * automates the suspension of normal MPL operations, runs the test, - * and resume. - * - * @{ - * @file ml_mputest.c - * @brief C wrapper to integrate the MPU Self Test wrapper in MPL. - * The main logic of the test and APIs can be found in mputest.c - */ - -#include <stdio.h> -#include <time.h> -#include <string.h> -#include <math.h> -#include <stdlib.h> - -#include "ml_mputest.h" - -#include "mlmath.h" -#include "mlinclude.h" -#include "ml.h" -#include "mlstates.h" -#include "mldl.h" -#include "mldl_cfg.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Globals -*/ -extern struct mldl_cfg *mputestCfgPtr; -extern signed char g_z_sign; - -/* - Prototypes -*/ -extern inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result); - -/** - * @brief An MPL wrapper for the main MPU Self Test API inv_factory_calibrate(). - * See inv_factory_calibrate() function for more details. - * - * @pre inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg - * data structure. - * On Windows, SetupPlatform() is also a madatory pre condition to - * ensure the accelerometer is properly configured before running the - * test. - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set thresholds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro_xxxx() functions. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * NOTE: for MPU6050 devices, this parameter is currently - * ignored. - * - * @return INV_SUCCESS or first non-zero error code otherwise. - */ -inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle, - unsigned char provide_result) -{ - INVENSENSE_FUNC_START; - inv_error_t firstError = INV_SUCCESS; - inv_error_t result; - unsigned char initState = inv_get_state(); - - if (initState < INV_STATE_DMP_OPENED) { - MPL_LOGE("Self Test cannot run before inv_dmp_open()\n"); - return INV_ERROR_SM_IMPROPER_STATE; - } - - /* obtain a pointer to the 'struct mldl_cfg' data structure. */ - mputestCfgPtr = inv_get_dl_config(); - - if(initState == INV_STATE_DMP_STARTED) { - result = inv_dmp_stop(); - ERROR_CHECK_FIRST(firstError, result); - } - - result = inv_factory_calibrate(mlsl_handle, provide_result); - ERROR_CHECK_FIRST(firstError, result); - - if(initState == INV_STATE_DMP_STARTED) { - result = inv_dmp_start(); - ERROR_CHECK_FIRST(firstError, result); - } - - return firstError; -} - -/** - * @brief Runs the MPU test at MPL runtime. - * If the DMP is operating, stops the DMP temporarely, - * runs the MPU Self Test, and re-starts the DMP. - * - * @return INV_SUCCESS or a non-zero error code otherwise. - */ -inv_error_t inv_self_test_run(void) -{ -#ifdef CONFIG_MPU_SENSORS_MPU3050 - return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE); -#else - return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); -#endif -} - -/** - * @brief Runs the MPU test for bias correction only at MPL runtime. - * If the DMP is operating, stops the DMP temporarely, - * runs the bias calculation routines, and re-starts the DMP. - * - * @return INV_SUCCESS or a non-zero error code otherwise. - */ -inv_error_t inv_self_test_bias_only(void) -{ - return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); -} - -/** - * @brief Set the orientation of the acceleroemter Z axis as it will be - * expected when running the MPU Self Test. - * Specifies the orientation of the accelerometer Z axis : Z axis - * pointing upwards or downwards. - * @param z_sign - * The sign of the accelerometer Z axis; valid values are +1 and - * -1 for +Z and -Z respectively. Any other value will cause the - * setting to be ignored and an error code to be returned. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign) -{ - if (z_sign != +1 && z_sign != -1) { - return INV_ERROR_INVALID_PARAMETER; - } - g_z_sign = z_sign; - return INV_SUCCESS; -} - - -#ifdef __cplusplus -} -#endif - -/** - * @} - */ - diff --git a/60xx/mlsdk/mllite/ml_mputest.h b/60xx/mlsdk/mllite/ml_mputest.h deleted file mode 100644 index 705d3cc..0000000 --- a/60xx/mlsdk/mllite/ml_mputest.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: ml_mputest.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *****************************************************************************/ - -#ifndef _INV_MPUTEST_H_ -#define _INV_MPUTEST_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" - -/* user APIs */ -inv_error_t inv_self_test_factory_calibrate( - void *mlsl_handle, unsigned char provide_result); -inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign); -inv_error_t inv_self_test_run(void); -inv_error_t inv_self_test_bias_only(void); - -/* other functions */ -#define inv_set_self_test_parameters inv_set_test_parameters - -#ifdef __cplusplus -} -#endif - -#endif /* _INV_MPUTEST_H_ */ - diff --git a/60xx/mlsdk/mllite/ml_stored_data.c b/60xx/mlsdk/mllite/ml_stored_data.c deleted file mode 100644 index 9107fe2..0000000 --- a/60xx/mlsdk/mllite/ml_stored_data.c +++ /dev/null @@ -1,1586 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: ml_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup ML_STORED_DATA - * - * @{ - * @file ml_stored_data.c - * @brief functions for reading and writing stored data sets. - * Typically, these functions process stored calibration data. - */ - -#include "ml_stored_data.h" -#include "ml.h" -#include "mlFIFO.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "compass.h" -#include "dmpKey.h" -#include "dmpDefault.h" -#include "mlstates.h" -#include "checksum.h" -#include "mlsupervisor.h" - -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-storeload" - -/* - Typedefs -*/ -typedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short); - -/* - Globals -*/ -extern struct inv_obj_t inv_obj; - -/* - Debugging Definitions - set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields - being read or stored in human-readable format. - When set to 0, the compiler will optimize and remove the printouts - from the code. -*/ -#define LOADCAL_DEBUG 0 -#define STORECAL_DEBUG 0 - -#define LOADCAL_LOG(...) \ - if(LOADCAL_DEBUG) \ - MPL_LOGI("LOADCAL: " __VA_ARGS__) -#define STORECAL_LOG(...) \ - if(STORECAL_DEBUG) \ - MPL_LOGI("STORECAL: " __VA_ARGS__) - -/** - * @brief Duplicate of the inv_temp_comp_find_bin function in the libmpl - * advanced algorithms library. To remove cross-dependency, for now, - * we reimplement the same function here. - * @param temp - * the temperature (1 count == 1 degree C). - */ -int FindTempBin(float temp) -{ - int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN); - if (bin < 0) - bin = 0; - if (bin > BINS - 1) - bin = BINS - 1; - return bin; -} - -/** - * @brief Returns the length of the <b>MPL internal calibration data</b>. - * Should be called before allocating the memory required to store - * this data to a file. - * This function returns the total size required to store the cal - * data including the header (4 bytes) and the checksum (2 bytes). - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return the length of the internal calibrated data format. - */ -unsigned int inv_get_cal_length(void) -{ - INVENSENSE_FUNC_START; - unsigned int length; - length = INV_CAL_HDR_LEN + // header - BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal - INV_CAL_ACCEL_LEN + // accel cal - INV_CAL_COMPASS_LEN + // compass cal - INV_CAL_CHK_LEN; // checksum - return length; -} - -/** - * @brief Loads a type 0 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature, - * - gyro biases for X, Y, Z axes. - * This calibration data would normally be produced by the MPU Self - * Test and its size is 18 bytes (header and checksum included). - * Calibration format type 0 is currently <b>NOT</b> used and - * is substituted by type 5 : inv_load_cal_V5(). - * - * @note This calibration data format is obsoleted and no longer supported - * by the rest of the MPL - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len) -{ - INVENSENSE_FUNC_START; - const short expLen = 18; - long newGyroData[3] = { 0, 0, 0 }; - float newTemp; - int bin; - - LOADCAL_LOG("Entering inv_load_cal_V0\n"); - - if (len != expLen) { - MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); - return INV_ERROR_FILE_READ; - } - - newTemp = (float)inv_decode_temperature( - (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; - LOADCAL_LOG("newTemp = %f\n", newTemp); - - newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); - if (newGyroData[0] > 32767L) - newGyroData[0] -= 65536L; - LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); - newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); - if (newGyroData[1] > 32767L) - newGyroData[1] -= 65536L; - LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); - newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); - if (newGyroData[2] > 32767L) - newGyroData[2] -= 65536L; - LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); - - bin = FindTempBin(newTemp); - LOADCAL_LOG("bin = %d", bin); - - inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; - LOADCAL_LOG("temp_data[%d][%d] = %f", - bin, inv_obj.temp_ptrs[bin], - inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); - inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = - ((float)newGyroData[0]) / 65536.f; - LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], - inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); - inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = - ((float)newGyroData[0]) / 65536.f; - LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], - inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); - inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = - ((float)newGyroData[0]) / 65536.f; - LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], - inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); - - inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; - LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); - - if (inv_obj.temp_ptrs[bin] == 0) - inv_obj.temp_valid_data[bin] = TRUE; - LOADCAL_LOG("temp_valid_data[%d] = %ld\n", - bin, inv_obj.temp_valid_data[bin]); - - inv_obj.got_no_motion_bias = TRUE; - LOADCAL_LOG("got_no_motion_bias = 1\n"); - inv_obj.cal_loaded_flag = TRUE; - LOADCAL_LOG("cal_loaded_flag = 1\n"); - - LOADCAL_LOG("Exiting inv_load_cal_V0\n"); - return INV_SUCCESS; -} - -/** - * @brief Loads a type 1 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature, - * - gyro biases for X, Y, Z axes, - * - accel biases for X, Y, Z axes. - * This calibration data would normally be produced by the MPU Self - * Test and its size is 24 bytes (header and checksum included). - * Calibration format type 1 is currently <b>NOT</b> used and - * is substituted by type 5 : inv_load_cal_V5(). - * - * @note In order to successfully work, the gyro bias must be stored - * expressed in 250 dps full scale (131.072 sensitivity). Other full - * scale range will produce unpredictable results in the gyro biases. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len) -{ - INVENSENSE_FUNC_START; - const short expLen = 24; - long newGyroData[3] = {0, 0, 0}; - long accelBias[3] = {0, 0, 0}; - float gyroBias[3] = {0, 0, 0}; - float newTemp = 0; - int bin; - - LOADCAL_LOG("Entering inv_load_cal_V1\n"); - - if (len != expLen) { - MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); - return INV_ERROR_FILE_READ; - } - - newTemp = (float)inv_decode_temperature( - (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; - LOADCAL_LOG("newTemp = %f\n", newTemp); - - newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); - if (newGyroData[0] > 32767L) - newGyroData[0] -= 65536L; - LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); - newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); - if (newGyroData[1] > 32767L) - newGyroData[1] -= 65536L; - LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]); - newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); - if (newGyroData[2] > 32767L) - newGyroData[2] -= 65536L; - LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); - - bin = FindTempBin(newTemp); - LOADCAL_LOG("bin = %d\n", bin); - - gyroBias[0] = ((float)newGyroData[0]) / 131.072f; - gyroBias[1] = ((float)newGyroData[1]) / 131.072f; - gyroBias[2] = ((float)newGyroData[2]) / 131.072f; - LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]); - LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]); - LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]); - - inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; - LOADCAL_LOG("temp_data[%d][%d] = %f", - bin, inv_obj.temp_ptrs[bin], - inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); - inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; - LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], - inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); - inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; - LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], - inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); - inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; - LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], - inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); - - inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; - LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); - - if (inv_obj.temp_ptrs[bin] == 0) - inv_obj.temp_valid_data[bin] = TRUE; - LOADCAL_LOG("temp_valid_data[%d] = %ld\n", - bin, inv_obj.temp_valid_data[bin]); - - /* load accel biases and apply immediately */ - accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]); - if (accelBias[0] > 32767) - accelBias[0] -= 65536L; - accelBias[0] = (long)((long long)accelBias[0] * 65536L * - inv_obj.accel_sens / 1073741824L); - LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]); - accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]); - if (accelBias[1] > 32767) - accelBias[1] -= 65536L; - accelBias[1] = (long)((long long)accelBias[1] * 65536L * - inv_obj.accel_sens / 1073741824L); - LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]); - accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]); - if (accelBias[2] > 32767) - accelBias[2] -= 65536L; - accelBias[2] = (long)((long long)accelBias[2] * 65536L * - inv_obj.accel_sens / 1073741824L); - LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]); - if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); - return inv_set_array(INV_ACCEL_BIAS, accelBias); - } - - inv_obj.got_no_motion_bias = TRUE; - LOADCAL_LOG("got_no_motion_bias = 1\n"); - inv_obj.cal_loaded_flag = TRUE; - LOADCAL_LOG("cal_loaded_flag = 1\n"); - - LOADCAL_LOG("Exiting inv_load_cal_V1\n"); - return INV_SUCCESS; -} - -/** - * @brief Loads a type 2 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature compensation : temperature data points, - * - temperature compensation : gyro biases data points for X, Y, - * and Z axes. - * - accel biases for X, Y, Z axes. - * This calibration data is produced internally by the MPL and its - * size is 2222 bytes (header and checksum included). - * Calibration format type 2 is currently <b>NOT</b> used and - * is substituted by type 4 : inv_load_cal_V4(). - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len) -{ - INVENSENSE_FUNC_START; - const short expLen = 2222; - long accel_bias[3]; - int ptr = INV_CAL_HDR_LEN; - - int i, j; - long long tmp; - - LOADCAL_LOG("Entering inv_load_cal_V2\n"); - - if (len != expLen) { - MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n", - expLen, len); - return INV_ERROR_FILE_READ; - } - - for (i = 0; i < BINS; i++) { - inv_obj.temp_ptrs[i] = 0; - inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); - inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); - inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); - inv_obj.temp_ptrs[i] += (int)calData[ptr++]; - LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); - } - for (i = 0; i < BINS; i++) { - inv_obj.temp_valid_data[i] = 0; - inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); - inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); - inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); - inv_obj.temp_valid_data[i] += (int)calData[ptr++]; - LOADCAL_LOG("temp_valid_data[%d] = %ld\n", - i, inv_obj.temp_valid_data[i]); - } - - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("temp_data[%d][%d] = %f\n", - i, j, inv_obj.temp_data[i][j]); - } - - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.x_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.y_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.z_gyro_temp_data[i][j]); - } - } - - /* read the accel biases */ - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - accel_bias[i] = (int32_t) t; - LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]); - } - - if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias)); - return inv_set_array(INV_ACCEL_BIAS, accel_bias); - } - - inv_obj.got_no_motion_bias = TRUE; - LOADCAL_LOG("got_no_motion_bias = 1\n"); - inv_obj.cal_loaded_flag = TRUE; - LOADCAL_LOG("cal_loaded_flag = 1\n"); - - LOADCAL_LOG("Exiting inv_load_cal_V2\n"); - return INV_SUCCESS; -} - -/** - * @brief Loads a type 3 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature compensation : temperature data points, - * - temperature compensation : gyro biases data points for X, Y, - * and Z axes. - * - accel biases for X, Y, Z axes. - * - compass biases for X, Y, Z axes and bias tracking algorithm - * mock-up. - * This calibration data is produced internally by the MPL and its - * size is 2429 bytes (header and checksum included). - * Calibration format type 3 is currently <b>NOT</b> used and - * is substituted by type 4 : inv_load_cal_V4(). - - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len) -{ - INVENSENSE_FUNC_START; - union doubleToLongLong { - double db; - unsigned long long ll; - } dToLL; - - const short expLen = 2429; - long bias[3]; - int i, j; - int ptr = INV_CAL_HDR_LEN; - long long tmp; - - LOADCAL_LOG("Entering inv_load_cal_V3\n"); - - if (len != expLen) { - MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n", - expLen, len); - return INV_ERROR_FILE_READ; - } - - for (i = 0; i < BINS; i++) { - inv_obj.temp_ptrs[i] = 0; - inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); - inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); - inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); - inv_obj.temp_ptrs[i] += (int)calData[ptr++]; - LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); - } - for (i = 0; i < BINS; i++) { - inv_obj.temp_valid_data[i] = 0; - inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); - inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); - inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); - inv_obj.temp_valid_data[i] += (int)calData[ptr++]; - LOADCAL_LOG("temp_valid_data[%d] = %ld\n", - i, inv_obj.temp_valid_data[i]); - } - - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("temp_data[%d][%d] = %f\n", - i, j, inv_obj.temp_data[i][j]); - } - } - - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.x_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.y_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.z_gyro_temp_data[i][j]); - } - } - - /* read the accel biases */ - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - bias[i] = (int32_t) t; - LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); - } - if (inv_set_array(INV_ACCEL_BIAS, bias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); - return inv_set_array(INV_ACCEL_BIAS, bias); - } - - /* read the compass biases */ - inv_obj.got_compass_bias = (int)calData[ptr++]; - inv_obj.got_init_compass_bias = (int)calData[ptr++]; - inv_obj.compass_state = (int)calData[ptr++]; - - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_bias_error[i] = (int32_t) t; - LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, - inv_obj.compass_bias_error[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.init_compass_bias[i] = (int32_t) t; - LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, - inv_obj.init_compass_bias[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_bias[i] = (int32_t) t; - LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); - } - for (i = 0; i < 18; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_peaks[i] = (int32_t) t; - LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); - } - for (i = 0; i < 3; i++) { - dToLL.ll = 0; - dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += (unsigned long long)calData[ptr++]; - - inv_obj.compass_bias_v[i] = dToLL.db; - LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); - } - for (i = 0; i < 9; i++) { - dToLL.ll = 0; - dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += (unsigned long long)calData[ptr++]; - - inv_obj.compass_bias_ptr[i] = dToLL.db; - LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, - inv_obj.compass_bias_ptr[i]); - } - - inv_obj.got_no_motion_bias = TRUE; - LOADCAL_LOG("got_no_motion_bias = 1\n"); - inv_obj.cal_loaded_flag = TRUE; - LOADCAL_LOG("cal_loaded_flag = 1\n"); - - LOADCAL_LOG("Exiting inv_load_cal_V3\n"); - return INV_SUCCESS; -} - -/** - * @brief Loads a type 4 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature compensation : temperature data points, - * - temperature compensation : gyro biases data points for X, Y, - * and Z axes. - * - accel biases for X, Y, Z axes. - * - compass biases for X, Y, Z axes, compass scale, and bias - * tracking algorithm mock-up. - * This calibration data is produced internally by the MPL and its - * size is 2777 bytes (header and checksum included). - * Calibration format type 4 is currently used and - * substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()). - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len) -{ - INVENSENSE_FUNC_START; - union doubleToLongLong { - double db; - unsigned long long ll; - } dToLL; - - const unsigned int expLen = 2782; - long bias[3]; - int ptr = INV_CAL_HDR_LEN; - int i, j; - long long tmp; - inv_error_t result; - - LOADCAL_LOG("Entering inv_load_cal_V4\n"); - - if (len != expLen) { - MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n", - expLen, len); - return INV_ERROR_FILE_READ; - } - - for (i = 0; i < BINS; i++) { - inv_obj.temp_ptrs[i] = 0; - inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); - inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); - inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); - inv_obj.temp_ptrs[i] += (int)calData[ptr++]; - LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); - } - for (i = 0; i < BINS; i++) { - inv_obj.temp_valid_data[i] = 0; - inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); - inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); - inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); - inv_obj.temp_valid_data[i] += (int)calData[ptr++]; - LOADCAL_LOG("temp_valid_data[%d] = %ld\n", - i, inv_obj.temp_valid_data[i]); - } - - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("temp_data[%d][%d] = %f\n", - i, j, inv_obj.temp_data[i][j]); - } - } - - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.x_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.y_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = 0; - tmp += 16777216LL * (long long)calData[ptr++]; - tmp += 65536LL * (long long)calData[ptr++]; - tmp += 256LL * (long long)calData[ptr++]; - tmp += (long long)calData[ptr++]; - if (tmp > 2147483648LL) { - tmp -= 4294967296LL; - } - inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; - LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.z_gyro_temp_data[i][j]); - } - } - - /* read the accel biases */ - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - bias[i] = (int32_t) t; - LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); - } - if (inv_set_array(INV_ACCEL_BIAS, bias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); - return inv_set_array(INV_ACCEL_BIAS, bias); - } - - /* read the compass biases */ - inv_reset_compass_calibration(); - - inv_obj.got_compass_bias = (int)calData[ptr++]; - LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); - inv_obj.got_init_compass_bias = (int)calData[ptr++]; - LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); - inv_obj.compass_state = (int)calData[ptr++]; - LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state); - - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_bias_error[i] = (int32_t) t; - LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, - inv_obj.compass_bias_error[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.init_compass_bias[i] = (int32_t) t; - LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, - inv_obj.init_compass_bias[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_bias[i] = (int32_t) t; - LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); - } - for (i = 0; i < 18; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_peaks[i] = (int32_t) t; - LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); - } - for (i = 0; i < 3; i++) { - dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += (unsigned long long)calData[ptr++]; - - inv_obj.compass_bias_v[i] = dToLL.db; - LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); - } - for (i = 0; i < 9; i++) { - dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += (unsigned long long)calData[ptr++]; - - inv_obj.compass_bias_ptr[i] = dToLL.db; - LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, - inv_obj.compass_bias_ptr[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = 0; - t += 16777216UL * ((uint32_t) calData[ptr++]); - t += 65536UL * ((uint32_t) calData[ptr++]); - t += 256u * ((uint32_t) calData[ptr++]); - t += (uint32_t) calData[ptr++]; - inv_obj.compass_scale[i] = (int32_t) t; - LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t); - } - for (i = 0; i < 6; i++) { - dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += (unsigned long long)calData[ptr++]; - - inv_obj.compass_prev_xty[i] = dToLL.db; - LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db); - } - for (i = 0; i < 36; i++) { - dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); - dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); - dToLL.ll += (unsigned long long)calData[ptr++]; - - inv_obj.compass_prev_m[i] = dToLL.db; - LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db); - } - - /* Load the compass offset flag and values */ - inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++]; - inv_obj.compass_offsets[0] = calData[ptr++]; - inv_obj.compass_offsets[1] = calData[ptr++]; - inv_obj.compass_offsets[2] = calData[ptr++]; - - inv_obj.compass_accuracy = calData[ptr++]; - /* push the compass offset values to the device */ - result = inv_set_compass_offset(); - - if (result == INV_SUCCESS) { - if (inv_compass_check_range() != INV_SUCCESS) { - MPL_LOGI("range check fail"); - inv_reset_compass_calibration(); - inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; - inv_set_compass_offset(); - } - } - - inv_obj.got_no_motion_bias = TRUE; - LOADCAL_LOG("got_no_motion_bias = 1\n"); - inv_obj.cal_loaded_flag = TRUE; - LOADCAL_LOG("cal_loaded_flag = 1\n"); - - LOADCAL_LOG("Exiting inv_load_cal_V4\n"); - return INV_SUCCESS; -} - -/** - * @brief Loads a type 5 set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * This calibrations data format stores values for (in order of - * appearance) : - * - temperature, - * - gyro biases for X, Y, Z axes, - * - accel biases for X, Y, Z axes. - * This calibration data would normally be produced by the MPU Self - * Test and its size is 36 bytes (header and checksum included). - * Calibration format type 5 is produced by the MPU Self Test and - * substitutes the type 1 : inv_load_cal_V1(). - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * @param len - * the length of the calibration - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len) -{ - INVENSENSE_FUNC_START; - const short expLen = 36; - long accelBias[3] = { 0, 0, 0 }; - float gyroBias[3] = { 0, 0, 0 }; - - int ptr = INV_CAL_HDR_LEN; - unsigned short temp; - float newTemp; - int bin; - int i; - - LOADCAL_LOG("Entering inv_load_cal_V5\n"); - - if (len != expLen) { - MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n", - expLen, len); - return INV_ERROR_FILE_READ; - } - - /* load the temperature */ - temp = (unsigned short)calData[ptr++] * (1L << 8); - temp += calData[ptr++]; - newTemp = (float)inv_decode_temperature(temp) / 65536.f; - LOADCAL_LOG("newTemp = %f\n", newTemp); - - /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */ - for (i = 0; i < 3; i++) { - int32_t tmp = 0; - tmp += (int32_t) calData[ptr++] * (1L << 24); - tmp += (int32_t) calData[ptr++] * (1L << 16); - tmp += (int32_t) calData[ptr++] * (1L << 8); - tmp += (int32_t) calData[ptr++]; - gyroBias[i] = (float)tmp / 65536.0f; - LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]); - } - /* find the temperature bin */ - bin = FindTempBin(newTemp); - - /* populate the temp comp data structure */ - inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; - LOADCAL_LOG("temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], newTemp); - - inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; - LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], gyroBias[0]); - inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; - LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], gyroBias[1]); - inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; - LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - bin, inv_obj.temp_ptrs[bin], gyroBias[2]); - inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; - LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); - - if (inv_obj.temp_ptrs[bin] == 0) - inv_obj.temp_valid_data[bin] = TRUE; - LOADCAL_LOG("temp_valid_data[%d] = %ld\n", - bin, inv_obj.temp_valid_data[bin]); - - /* load accel biases (represented in 2 ^ 16 == 1 gee) - and apply immediately */ - for (i = 0; i < 3; i++) { - int32_t tmp = 0; - tmp += (int32_t) calData[ptr++] * (1L << 24); - tmp += (int32_t) calData[ptr++] * (1L << 16); - tmp += (int32_t) calData[ptr++] * (1L << 8); - tmp += (int32_t) calData[ptr++]; - accelBias[i] = (long)tmp; - LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]); - } - if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { - LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); - return inv_set_array(INV_ACCEL_BIAS, accelBias); - } - - inv_obj.got_no_motion_bias = TRUE; - LOADCAL_LOG("got_no_motion_bias = 1\n"); - inv_obj.cal_loaded_flag = TRUE; - LOADCAL_LOG("cal_loaded_flag = 1\n"); - - LOADCAL_LOG("Exiting inv_load_cal_V5\n"); - return INV_SUCCESS; -} - -/** - * @brief Loads a set of calibration data. - * It parses a binary data set containing calibration data. - * The binary data set is intended to be loaded from a file. - * - * @pre inv_dmp_open() - * @ifnot MPL_MF - * or inv_open_low_power_pedometer() - * or inv_eis_open_dmp() - * @endif - * must have been called. - * - * @param calData - * A pointer to an array of bytes to be parsed. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_cal(unsigned char *calData) -{ - INVENSENSE_FUNC_START; - int calType = 0; - int len = 0; - int ptr; - uint32_t chk = 0; - uint32_t cmp_chk = 0; - - tMLLoadFunc loaders[] = { - inv_load_cal_V0, - inv_load_cal_V1, - inv_load_cal_V2, - inv_load_cal_V3, - inv_load_cal_V4, - inv_load_cal_V5 - }; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - /* read the header (type and len) - len is the total record length including header and checksum */ - len = 0; - len += 16777216L * ((int)calData[0]); - len += 65536L * ((int)calData[1]); - len += 256 * ((int)calData[2]); - len += (int)calData[3]; - - calType = ((int)calData[4]) * 256 + ((int)calData[5]); - if (calType > 5) { - MPL_LOGE("Unsupported calibration file format %d. " - "Valid types 0..5\n", calType); - return INV_ERROR_INVALID_PARAMETER; - } - - /* check the checksum */ - chk = 0; - ptr = len - INV_CAL_CHK_LEN; - - chk += 16777216L * ((uint32_t) calData[ptr++]); - chk += 65536L * ((uint32_t) calData[ptr++]); - chk += 256 * ((uint32_t) calData[ptr++]); - chk += (uint32_t) calData[ptr++]; - - cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN, - len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); - - if (chk != cmp_chk) { - return INV_ERROR_CALIBRATION_CHECKSUM; - } - - /* call the proper method to read in the data */ - return loaders[calType] (calData, len); -} - -/** - * @brief Stores a set of calibration data. - * It generates a binary data set containing calibration data. - * The binary data set is intended to be stored into a file. - * - * @pre inv_dmp_open() - * - * @param calData - * A pointer to an array of bytes to be stored. - * @param length - * The amount of bytes available in the array. - * - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_store_cal(unsigned char *calData, int length) -{ - INVENSENSE_FUNC_START; - int ptr = 0; - int i = 0; - int j = 0; - long long tmp; - uint32_t chk; - long bias[3]; - //unsigned char state; - union doubleToLongLong { - double db; - unsigned long long ll; - } dToLL; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - STORECAL_LOG("Entering inv_store_cal\n"); - - // length - calData[0] = (unsigned char)((length >> 24) & 0xff); - calData[1] = (unsigned char)((length >> 16) & 0xff); - calData[2] = (unsigned char)((length >> 8) & 0xff); - calData[3] = (unsigned char)(length & 0xff); - STORECAL_LOG("calLen = %d\n", length); - - // calibration data format type - calData[4] = 0; - calData[5] = 4; - STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]); - - // data - ptr = 6; - for (i = 0; i < BINS; i++) { - tmp = (int)inv_obj.temp_ptrs[i]; - calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); - calData[ptr++] = (unsigned char)(tmp & 0xff); - STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp); - } - - for (i = 0; i < BINS; i++) { - tmp = (int)inv_obj.temp_valid_data[i]; - calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); - calData[ptr++] = (unsigned char)(tmp & 0xff); - STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp); - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f); - if (tmp < 0) { - tmp += 4294967296LL; - } - calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); - calData[ptr++] = (unsigned char)(tmp & 0xff); - STORECAL_LOG("temp_data[%d][%d] = %f\n", - i, j, inv_obj.temp_data[i][j]); - } - } - - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f); - if (tmp < 0) { - tmp += 4294967296LL; - } - calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); - calData[ptr++] = (unsigned char)(tmp & 0xff); - STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.x_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f); - if (tmp < 0) { - tmp += 4294967296LL; - } - calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); - calData[ptr++] = (unsigned char)(tmp & 0xff); - STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.y_gyro_temp_data[i][j]); - } - } - for (i = 0; i < BINS; i++) { - for (j = 0; j < PTS_PER_BIN; j++) { - tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f); - if (tmp < 0) { - tmp += 4294967296LL; - } - calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); - calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); - calData[ptr++] = (unsigned char)(tmp & 0xff); - STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", - i, j, inv_obj.z_gyro_temp_data[i][j]); - } - } - - inv_get_array(INV_ACCEL_BIAS, bias); - - /* write the accel biases */ - for (i = 0; i < 3; i++) { - uint32_t t = (uint32_t) bias[i]; - calData[ptr++] = (unsigned char)((t >> 24) & 0xff); - calData[ptr++] = (unsigned char)((t >> 16) & 0xff); - calData[ptr++] = (unsigned char)((t >> 8) & 0xff); - calData[ptr++] = (unsigned char)(t & 0xff); - STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); - } - - /* write the compass calibration state */ - calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias); - STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); - calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias); - STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); - if (inv_obj.compass_state == SF_UNCALIBRATED) { - calData[ptr++] = SF_UNCALIBRATED; - } else { - calData[ptr++] = SF_STARTUP_SETTLE; - } - STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state); - - for (i = 0; i < 3; i++) { - uint32_t t = (uint32_t) inv_obj.compass_bias_error[i]; - calData[ptr++] = (unsigned char)((t >> 24) & 0xff); - calData[ptr++] = (unsigned char)((t >> 16) & 0xff); - calData[ptr++] = (unsigned char)((t >> 8) & 0xff); - calData[ptr++] = (unsigned char)(t & 0xff); - STORECAL_LOG("compass_bias_error[%d] = %ld\n", - i, inv_obj.compass_bias_error[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = (uint32_t) inv_obj.init_compass_bias[i]; - calData[ptr++] = (unsigned char)((t >> 24) & 0xff); - calData[ptr++] = (unsigned char)((t >> 16) & 0xff); - calData[ptr++] = (unsigned char)((t >> 8) & 0xff); - calData[ptr++] = (unsigned char)(t & 0xff); - STORECAL_LOG("init_compass_bias[%d] = %ld\n", i, - inv_obj.init_compass_bias[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = (uint32_t) inv_obj.compass_bias[i]; - calData[ptr++] = (unsigned char)((t >> 24) & 0xff); - calData[ptr++] = (unsigned char)((t >> 16) & 0xff); - calData[ptr++] = (unsigned char)((t >> 8) & 0xff); - calData[ptr++] = (unsigned char)(t & 0xff); - STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); - } - for (i = 0; i < 18; i++) { - uint32_t t = (uint32_t) inv_obj.compass_peaks[i]; - calData[ptr++] = (unsigned char)((t >> 24) & 0xff); - calData[ptr++] = (unsigned char)((t >> 16) & 0xff); - calData[ptr++] = (unsigned char)((t >> 8) & 0xff); - calData[ptr++] = (unsigned char)(t & 0xff); - STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); - } - for (i = 0; i < 3; i++) { - dToLL.db = inv_obj.compass_bias_v[i]; - calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); - calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); - STORECAL_LOG("compass_bias_v[%d] = %lf\n", i, - inv_obj.compass_bias_v[i]); - } - for (i = 0; i < 9; i++) { - dToLL.db = inv_obj.compass_bias_ptr[i]; - calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); - calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); - STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i, - inv_obj.compass_bias_ptr[i]); - } - for (i = 0; i < 3; i++) { - uint32_t t = (uint32_t) inv_obj.compass_scale[i]; - calData[ptr++] = (unsigned char)((t >> 24) & 0xff); - calData[ptr++] = (unsigned char)((t >> 16) & 0xff); - calData[ptr++] = (unsigned char)((t >> 8) & 0xff); - calData[ptr++] = (unsigned char)(t & 0xff); - STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]); - } - for (i = 0; i < 6; i++) { - dToLL.db = inv_obj.compass_prev_xty[i]; - calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); - calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); - STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i, - inv_obj.compass_prev_xty[i]); - } - for (i = 0; i < 36; i++) { - dToLL.db = inv_obj.compass_prev_m[i]; - calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); - calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); - calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); - STORECAL_LOG("compass_prev_m[%d] = %lf\n", i, - inv_obj.compass_prev_m[i]); - } - - /* store the compass offsets and validity flag */ - calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID]; - calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0]; - calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1]; - calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2]; - - /* store the compass accuracy */ - calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy); - - /* add a checksum */ - chk = inv_checksum(calData + INV_CAL_HDR_LEN, - length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); - calData[ptr++] = (unsigned char)((chk >> 24) & 0xff); - calData[ptr++] = (unsigned char)((chk >> 16) & 0xff); - calData[ptr++] = (unsigned char)((chk >> 8) & 0xff); - calData[ptr++] = (unsigned char)(chk & 0xff); - - STORECAL_LOG("Exiting inv_store_cal\n"); - return INV_SUCCESS; -} - -/** - * @brief Load a calibration file. - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return 0 or error code. - */ -inv_error_t inv_load_calibration(void) -{ - unsigned char *calData; - inv_error_t result; - unsigned int length; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - result = inv_serial_get_cal_length(&length); - if (result == INV_ERROR_FILE_OPEN) { - MPL_LOGI("Calibration data not loaded\n"); - return INV_SUCCESS; - } - if (result || length <= 0) { - MPL_LOGE("Could not get file calibration length - " - "error %d - aborting\n", result); - return result; - } - calData = (unsigned char *)inv_malloc(length); - if (!calData) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", length); - return INV_ERROR_MEMORY_EXAUSTED; - } - result = inv_serial_read_cal(calData, length); - if (result) { - MPL_LOGE("Could not read the calibration data from file - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - - } - result = inv_load_cal(calData); - if (result) { - MPL_LOGE("Could not load the calibration data - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - - } - - - -free_mem_n_exit: - inv_free(calData); - return INV_SUCCESS; -} - -/** - * @brief Store runtime calibration data to a file - * - * @pre Must be in INV_STATE_DMP_OPENED state. - * inv_dmp_open() or inv_dmp_stop() must have been called. - * inv_dmp_start() and inv_dmp_close() must have <b>NOT</b> - * been called. - * - * @return 0 or error code. - */ -inv_error_t inv_store_calibration(void) -{ - unsigned char *calData; - inv_error_t result; - unsigned int length; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - length = inv_get_cal_length(); - calData = (unsigned char *)inv_malloc(length); - if (!calData) { - MPL_LOGE("Could not allocate buffer of %d bytes - " - "aborting\n", length); - return INV_ERROR_MEMORY_EXAUSTED; - } - result = inv_store_cal(calData, length); - if (result) { - MPL_LOGE("Could not store calibrated data on file - " - "error %d - aborting\n", result); - goto free_mem_n_exit; - - } - result = inv_serial_write_cal(calData, length); - if (result) { - MPL_LOGE("Could not write calibration data - " "error %d\n", result); - goto free_mem_n_exit; - - } - - - -free_mem_n_exit: - inv_free(calData); - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/ml_stored_data.h b/60xx/mlsdk/mllite/ml_stored_data.h deleted file mode 100644 index 74c2b7c..0000000 --- a/60xx/mlsdk/mllite/ml_stored_data.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/******************************************************************************* - * - * $Id:$ - * - ******************************************************************************/ - -#ifndef INV_STORED_DATA_H -#define INV_STORED_DATA_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Includes. -*/ - -#include "mltypes.h" - -/* - Defines -*/ -#define INV_CAL_ACCEL_LEN (12) -#define INV_CAL_COMPASS_LEN (555 + 5) -#define INV_CAL_HDR_LEN (6) -#define INV_CAL_CHK_LEN (4) - -/* - APIs -*/ - inv_error_t inv_load_calibration(void); - inv_error_t inv_store_calibration(void); - -/* - Other prototypes -*/ - inv_error_t inv_load_cal(unsigned char *calData); - inv_error_t inv_store_cal(unsigned char *calData, int length); - unsigned int inv_get_cal_length(void); - -#ifdef __cplusplus -} -#endif -#endif /* INV_STORED_DATA_H */ diff --git a/60xx/mlsdk/mllite/mlarray.c b/60xx/mlsdk/mllite/mlarray.c deleted file mode 100644 index 6ae85e0..0000000 --- a/60xx/mlsdk/mllite/mlarray.c +++ /dev/null @@ -1,2524 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: mlarray.c 5085 2011-04-08 22:25:14Z phickey $ - * - *****************************************************************************/ - -/** - * @defgroup ML - * @{ - * @file mlarray.c - * @brief APIs to read different data sets from FIFO. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ -#include "ml.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "mlMathFunc.h" -#include "mlmath.h" -#include "mlstates.h" -#include "mlFIFO.h" -#include "mlsupervisor.h" -#include "mldl.h" -#include "dmpKey.h" -#include "compass.h" - -/** - * @brief inv_get_gyro is used to get the most recent gyroscope measurement. - * The argument array elements are ordered X,Y,Z. - * The values are scaled at 1 dps = 2^16 LSBs. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - - /* inv_get_gyro implemented in mlFIFO.c */ - -/** - * @brief inv_get_accel is used to get the most recent - * accelerometer measurement. - * The argument array elements are ordered X,Y,Z. - * The values are scaled in units of g (gravity), - * where 1 g = 2^16 LSBs. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - /* inv_get_accel implemented in mlFIFO.c */ - -/** - * @brief inv_get_temperature is used to get the most recent - * temperature measurement. - * The argument array should only have one element. - * The value is in units of deg C (degrees Celsius), where - * 2^16 LSBs = 1 deg C. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to the data to be passed back to the user. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - /* inv_get_temperature implemented in mlFIFO.c */ - -/** - * @brief inv_get_rot_mat is used to get the rotation matrix - * representation of the current sensor fusion solution. - * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, - * R33, representing the matrix: - * <center>R11 R12 R13</center> - * <center>R21 R22 R23</center> - * <center>R31 R32 R33</center> - * Values are scaled, where 1.0 = 2^30 LSBs. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_rot_mat(long *data) -{ - inv_error_t result = INV_SUCCESS; - long qdata[4]; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - inv_get_quaternion(qdata); - inv_quaternion_to_rotation(qdata, data); - - return result; -} - -/** - * @brief inv_get_quaternion is used to get the quaternion representation - * of the current sensor fusion solution. - * The values are scaled where 1.0 = 2^30 LSBs. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 4 cells long </b>. - * - * @return INV_SUCCESS if the command is successful; an ML error code otherwise. - */ - /* inv_get_quaternion implemented in mlFIFO.c */ - -/** - * @brief inv_get_linear_accel is used to get an estimate of linear - * acceleration, based on the most recent accelerometer measurement - * and sensor fusion solution. - * The values are scaled where 1 g (gravity) = 2^16 LSBs. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - /* inv_get_linear_accel implemented in mlFIFO.c */ - -/** - * @brief inv_get_linear_accel_in_world is used to get an estimate of - * linear acceleration, in the world frame, based on the most - * recent accelerometer measurement and sensor fusion solution. - * The values are scaled where 1 g (gravity) = 2^16 LSBs. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - /* inv_get_linear_accel_in_world implemented in mlFIFO.c */ - -/** - * @brief inv_get_gravity is used to get an estimate of the body frame - * gravity vector, based on the most recent sensor fusion solution. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - /* inv_get_gravity implemented in mlFIFO.c */ - -/** - * @internal - * @brief inv_get_angular_velocity is used to get an estimate of the body - * frame angular velocity, which is computed from the current and - * previous sensor fusion solutions. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_angular_velocity(long *data) -{ - - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - /* not implemented. old (invalid) implementation: - if ( inv_get_state() < INV_STATE_DMP_OPENED ) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - data[0] = inv_obj.ang_v_body[0]; - data[1] = inv_obj.ang_v_body[1]; - data[2] = inv_obj.ang_v_body[2]; - - return result; - */ -} - -/** - * @brief inv_get_euler_angles is used to get the Euler angle representation - * of the current sensor fusion solution. - * Euler angles may follow various conventions. This function is equivelant - * to inv_get_euler_angles_x, refer to inv_get_euler_angles_x for more - * information. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_euler_angles(long *data) -{ - return inv_get_euler_angles_x(data); -} - -/** - * @brief inv_get_euler_angles_x is used to get the Euler angle representation - * of the current sensor fusion solution. - * Euler angles are returned according to the X convention. - * This is typically the convention used for mobile devices where the X - * axis is the width of the screen, Y axis is the height, and Z the - * depth. In this case roll is defined as the rotation around the X - * axis of the device. - * <TABLE> - * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR> - * <TR><TD> 0 </TD><TD>Roll </TD><TD>X axis </TD></TR> - * <TR><TD> 1 </TD><TD>Pitch </TD><TD>Y axis </TD></TR> - * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * - * Values are scaled where 1.0 = 2^16. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_euler_angles_x(long *data) -{ - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_rot_mat_float(rotMatrix); - tmp = rotMatrix[6]; - if (tmp > 1.0f) { - tmp = 1.0f; - } - if (tmp < -1.0f) { - tmp = -1.0f; - } - data[0] = - (long)((float) - (atan2f(rotMatrix[7], rotMatrix[8]) * 57.29577951308) * - 65536L); - data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L); - data[2] = - (long)((float) - (atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308) * - 65536L); - return result; -} - -/** - * @brief inv_get_euler_angles_y is used to get the Euler angle representation - * of the current sensor fusion solution. - * Euler angles are returned according to the Y convention. - * This convention is typically used in augmented reality applications, - * where roll is defined as the rotation around the axis along the - * height of the screen of a mobile device, namely the Y axis. - * <TABLE> - * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR> - * <TR><TD> 0 </TD><TD>Roll </TD><TD>Y axis </TD></TR> - * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR> - * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * - * Values are scaled where 1.0 = 2^16. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_euler_angles_y(long *data) -{ - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_rot_mat_float(rotMatrix); - tmp = rotMatrix[7]; - if (tmp > 1.0f) { - tmp = 1.0f; - } - if (tmp < -1.0f) { - tmp = -1.0f; - } - data[0] = - (long)((float) - (atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308f) * - 65536L); - data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L); - data[2] = - (long)((float) - (atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308f) * - 65536L); - return result; -} - -/** @brief inv_get_euler_angles_z is used to get the Euler angle representation - * of the current sensor fusion solution. - * This convention is mostly used in application involving the use - * of a camera, typically placed on the back of a mobile device, that - * is along the Z axis. In this convention roll is defined as the - * rotation around the Z axis. - * Euler angles are returned according to the Y convention. - * <TABLE> - * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR> - * <TR><TD> 0 </TD><TD>Roll </TD><TD>Z axis </TD></TR> - * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR> - * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Y axis </TD></TR> - * </TABLE> - * - * Values are scaled where 1.0 = 2^16. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ - -inv_error_t inv_get_euler_angles_z(long *data) -{ - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_rot_mat_float(rotMatrix); - tmp = rotMatrix[8]; - if (tmp > 1.0f) { - tmp = 1.0f; - } - if (tmp < -1.0f) { - tmp = -1.0f; - } - data[0] = - (long)((float) - (atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308) * - 65536L); - data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L); - data[2] = - (long)((float) - (atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308) * - 65536L); - return result; -} - -/** - * @brief inv_get_gyro_temp_slope is used to get is used to get the temperature - * compensation algorithm's estimate of the gyroscope bias temperature - * coefficient. - * The argument array elements are ordered X,Y,Z. - * Values are in units of dps per deg C (degrees per second per degree - * Celcius). Values are scaled so that 1 dps per deg C = 2^16 LSBs. - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_gyro_temp_slope(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) { - data[0] = (long)(inv_obj.x_gyro_coef[1] * 65536.0f); - data[1] = (long)(inv_obj.y_gyro_coef[1] * 65536.0f); - data[2] = (long)(inv_obj.z_gyro_coef[1] * 65536.0f); - } else { - data[0] = inv_obj.temp_slope[0]; - data[1] = inv_obj.temp_slope[1]; - data[2] = inv_obj.temp_slope[2]; - } - return result; -} - -/** - * @brief inv_get_gyro_bias is used to get the gyroscope biases. - * The argument array elements are ordered X,Y,Z. - * The values are scaled such that 1 dps = 2^16 LSBs. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_gyro_bias(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - data[0] = inv_obj.gyro_bias[0]; - data[1] = inv_obj.gyro_bias[1]; - data[2] = inv_obj.gyro_bias[2]; - - return result; -} - -/** - * @brief inv_get_accel_bias is used to get the accelerometer baises. - * The argument array elements are ordered X,Y,Z. - * The values are scaled such that 1 g (gravity) = 2^16 LSBs. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_accel_bias(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - data[0] = inv_obj.accel_bias[0]; - data[1] = inv_obj.accel_bias[1]; - data[2] = inv_obj.accel_bias[2]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_bias is used to get Magnetometer Bias - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_bias(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - data[0] = - inv_obj.compass_bias[0] + - (long)((long long)inv_obj.init_compass_bias[0] * inv_obj.compass_sens / - 16384); - data[1] = - inv_obj.compass_bias[1] + - (long)((long long)inv_obj.init_compass_bias[1] * inv_obj.compass_sens / - 16384); - data[2] = - inv_obj.compass_bias[2] + - (long)((long long)inv_obj.init_compass_bias[2] * inv_obj.compass_sens / - 16384); - - return result; -} - -/** - * @brief inv_get_gyro_and_accel_sensor is used to get the most recent set of all sensor data. - * The argument array elements are ordered gyroscope X,Y, and Z, - * accelerometer X, Y, and Z, and magnetometer X,Y, and Z. - * \if UMPL The magnetometer elements are not populated in UMPL. \endif - * The gyroscope and accelerometer data is not scaled or offset, it is - * copied directly from the sensor registers. - * In the case of accelerometers with 8-bit output resolution, the data - * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g - * full scale range - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - /* inv_get_gyro_and_accel_sensor implemented in mlFIFO.c */ - -/** - * @cond MPL - * @brief inv_get_mag_raw_data is used to get Raw magnetometer data. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_raw_data(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.compass_sensor_data[0]; - data[1] = inv_obj.compass_sensor_data[1]; - data[2] = inv_obj.compass_sensor_data[2]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_magnetometer is used to get magnetometer data. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_magnetometer(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]; - data[1] = inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]; - data[2] = inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_pressure is used to get Pressure data. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to data to be passed back to the user. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_pressure(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.pressure; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_heading is used to get heading from Rotation Matrix. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to data to be passed back to the user. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ - -inv_error_t inv_get_heading(long *data) -{ - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_get_rot_mat_float(rotMatrix); - if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) { - tmp = - (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 - - 90.0f); - } else { - tmp = - (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 + - 90.0f); - } - if (tmp < 0) { - tmp += 360.0f; - } - data[0] = (long)((360 - tmp) * 65536.0f); - - return result; -} - -/** - * @brief inv_get_gyro_cal_matrix is used to get the gyroscope - * calibration matrix. The gyroscope calibration matrix defines the relationship - * between the gyroscope sensor axes and the sensor fusion solution axes. - * Calibration matrix data members will have a value of 1, 0, or -1. - * The matrix has members - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_gyro_cal_matrix(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.gyro_cal[0]; - data[1] = inv_obj.gyro_cal[1]; - data[2] = inv_obj.gyro_cal[2]; - data[3] = inv_obj.gyro_cal[3]; - data[4] = inv_obj.gyro_cal[4]; - data[5] = inv_obj.gyro_cal[5]; - data[6] = inv_obj.gyro_cal[6]; - data[7] = inv_obj.gyro_cal[7]; - data[8] = inv_obj.gyro_cal[8]; - - return result; -} - -/** - * @brief inv_get_accel_cal_matrix is used to get the accelerometer - * calibration matrix. - * Calibration matrix data members will have a value of 1, 0, or -1. - * The matrix has members - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_accel_cal_matrix(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.accel_cal[0]; - data[1] = inv_obj.accel_cal[1]; - data[2] = inv_obj.accel_cal[2]; - data[3] = inv_obj.accel_cal[3]; - data[4] = inv_obj.accel_cal[4]; - data[5] = inv_obj.accel_cal[5]; - data[6] = inv_obj.accel_cal[6]; - data[7] = inv_obj.accel_cal[7]; - data[8] = inv_obj.accel_cal[8]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_cal_matrix is used to get magnetometer calibration matrix. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_cal_matrix(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.compass_cal[0]; - data[1] = inv_obj.compass_cal[1]; - data[2] = inv_obj.compass_cal[2]; - data[3] = inv_obj.compass_cal[3]; - data[4] = inv_obj.compass_cal[4]; - data[5] = inv_obj.compass_cal[5]; - data[6] = inv_obj.compass_cal[6]; - data[7] = inv_obj.compass_cal[7]; - data[8] = inv_obj.compass_cal[8]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_bias_error is used to get magnetometer Bias error. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_bias_error(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - if (inv_obj.large_field == 0) { - data[0] = inv_obj.compass_bias_error[0]; - data[1] = inv_obj.compass_bias_error[1]; - data[2] = inv_obj.compass_bias_error[2]; - } else { - data[0] = P_INIT; - data[1] = P_INIT; - data[2] = P_INIT; - } - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_scale is used to get magnetometer scale. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_scale(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.compass_scale[0]; - data[1] = inv_obj.compass_scale[1]; - data[2] = inv_obj.compass_scale[2]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_local_field is used to get local magnetic field data. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -inv_error_t inv_get_local_field(long *data) -{ - inv_error_t result = INV_SUCCESS; - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = inv_obj.local_field[0]; - data[1] = inv_obj.local_field[1]; - data[2] = inv_obj.local_field[2]; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_relative_quaternion is used to get relative quaternion. - * - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 4 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - * @endcond - */ -/* inv_get_relative_quaternion implemented in mlFIFO.c */ - -/** - * @brief inv_get_gyro_float is used to get the most recent gyroscope measurement. - * The argument array elements are ordered X,Y,Z. - * The values are in units of dps (degrees per second). - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_gyro_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - long ldata[3]; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_get_gyro(ldata); - data[0] = (float)ldata[0] / 65536.0f; - data[1] = (float)ldata[1] / 65536.0f; - data[2] = (float)ldata[2] / 65536.0f; - - return result; -} - -/** - * @internal - * @brief inv_get_angular_velocity_float is used to get an array of three data points representing the angular - * velocity as derived from <b>both</b> gyroscopes and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled, to fuse data from - * the gyroscope and accelerometer device, appropriately scaled and - * oriented according to the respective mounting matrices. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_angular_velocity_float(float *data) -{ - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - /* not implemented. old (invalid) implementation: - return inv_get_gyro_float(data); - */ -} - -/** - * @brief inv_get_accel_float is used to get the most recent accelerometer measurement. - * The argument array elements are ordered X,Y,Z. - * The values are in units of g (gravity). - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ - /* inv_get_accel_float implemented in mlFIFO.c */ - -/** - * @brief inv_get_temperature_float is used to get the most recent - * temperature measurement. - * The argument array should only have one element. - * The value is in units of deg C (degrees Celsius). - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to data to be passed back to the user. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_temperature_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - long ldata[1]; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_temperature(ldata); - data[0] = (float)ldata[0] / 65536.0f; - - return result; -} - -/** - * @brief inv_get_rot_mat_float is used to get an array of nine data points representing the rotation - * matrix generated from all available sensors. - * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, - * R33, representing the matrix: - * <center>R11 R12 R13</center> - * <center>R21 R22 R23</center> - * <center>R31 R32 R33</center> - * <b>Please refer to the "9-Axis Sensor Fusion Application Note" document, - * section 7 "Sensor Fusion Output", for details regarding rotation - * matrix output</b>. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_rot_mat_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - { - long qdata[4], rdata[9]; - inv_get_quaternion(qdata); - inv_quaternion_to_rotation(qdata, rdata); - data[0] = (float)rdata[0] / 1073741824.0f; - data[1] = (float)rdata[1] / 1073741824.0f; - data[2] = (float)rdata[2] / 1073741824.0f; - data[3] = (float)rdata[3] / 1073741824.0f; - data[4] = (float)rdata[4] / 1073741824.0f; - data[5] = (float)rdata[5] / 1073741824.0f; - data[6] = (float)rdata[6] / 1073741824.0f; - data[7] = (float)rdata[7] / 1073741824.0f; - data[8] = (float)rdata[8] / 1073741824.0f; - } - - return result; -} - -/** - * @brief inv_get_quaternion_float is used to get the quaternion representation - * of the current sensor fusion solution. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 4 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an ML error code otherwise. - */ - /* inv_get_quaternion_float implemented in mlFIFO.c */ - -/** - * @brief inv_get_linear_accel_float is used to get an estimate of linear - * acceleration, based on the most recent accelerometer measurement - * and sensor fusion solution. - * The values are in units of g (gravity). - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_linear_accel_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - long ldata[3]; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_linear_accel(ldata); - data[0] = (float)ldata[0] / 65536.0f; - data[1] = (float)ldata[1] / 65536.0f; - data[2] = (float)ldata[2] / 65536.0f; - - return result; -} - -/** - * @brief inv_get_linear_accel_in_world_float is used to get an estimate of - * linear acceleration, in the world frame, based on the most - * recent accelerometer measurement and sensor fusion solution. - * The values are in units of g (gravity). - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_linear_accel_in_world_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - long ldata[3]; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_linear_accel_in_world(ldata); - data[0] = (float)ldata[0] / 65536.0f; - data[1] = (float)ldata[1] / 65536.0f; - data[2] = (float)ldata[2] / 65536.0f; - - return result; -} - -/** - * @brief inv_get_gravity_float is used to get an estimate of the body frame - * gravity vector, based on the most recent sensor fusion solution. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long at least</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_gravity_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - long ldata[3]; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_get_gravity(ldata); - data[0] = (float)ldata[0] / 65536.0f; - data[1] = (float)ldata[1] / 65536.0f; - data[2] = (float)ldata[2] / 65536.0f; - - return result; -} - -/** - * @brief inv_get_gyro_cal_matrix_float is used to get the gyroscope - * calibration matrix. The gyroscope calibration matrix defines the relationship - * between the gyroscope sensor axes and the sensor fusion solution axes. - * Calibration matrix data members will have a value of 1.0, 0, or -1.0. - * The matrix has members - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_gyro_cal_matrix_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.gyro_cal[0] / 1073741824.0f; - data[1] = (float)inv_obj.gyro_cal[1] / 1073741824.0f; - data[2] = (float)inv_obj.gyro_cal[2] / 1073741824.0f; - data[3] = (float)inv_obj.gyro_cal[3] / 1073741824.0f; - data[4] = (float)inv_obj.gyro_cal[4] / 1073741824.0f; - data[5] = (float)inv_obj.gyro_cal[5] / 1073741824.0f; - data[6] = (float)inv_obj.gyro_cal[6] / 1073741824.0f; - data[7] = (float)inv_obj.gyro_cal[7] / 1073741824.0f; - data[8] = (float)inv_obj.gyro_cal[8] / 1073741824.0f; - - return result; -} - -/** - * @brief inv_get_accel_cal_matrix_float is used to get the accelerometer - * calibration matrix. - * Calibration matrix data members will have a value of 1.0, 0, or -1.0. - * The matrix has members - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ - -inv_error_t inv_get_accel_cal_matrix_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.accel_cal[0] / 1073741824.0f; - data[1] = (float)inv_obj.accel_cal[1] / 1073741824.0f; - data[2] = (float)inv_obj.accel_cal[2] / 1073741824.0f; - data[3] = (float)inv_obj.accel_cal[3] / 1073741824.0f; - data[4] = (float)inv_obj.accel_cal[4] / 1073741824.0f; - data[5] = (float)inv_obj.accel_cal[5] / 1073741824.0f; - data[6] = (float)inv_obj.accel_cal[6] / 1073741824.0f; - data[7] = (float)inv_obj.accel_cal[7] / 1073741824.0f; - data[8] = (float)inv_obj.accel_cal[8] / 1073741824.0f; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_cal_matrix_float is used to get an array of nine data points - * representing the calibration matrix for the compass: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_cal_matrix_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.compass_cal[0] / 1073741824.0f; - data[1] = (float)inv_obj.compass_cal[1] / 1073741824.0f; - data[2] = (float)inv_obj.compass_cal[2] / 1073741824.0f; - data[3] = (float)inv_obj.compass_cal[3] / 1073741824.0f; - data[4] = (float)inv_obj.compass_cal[4] / 1073741824.0f; - data[5] = (float)inv_obj.compass_cal[5] / 1073741824.0f; - data[6] = (float)inv_obj.compass_cal[6] / 1073741824.0f; - data[7] = (float)inv_obj.compass_cal[7] / 1073741824.0f; - data[8] = (float)inv_obj.compass_cal[8] / 1073741824.0f; - return result; -} - -/** - * @brief inv_get_gyro_temp_slope_float is used to get the temperature - * compensation algorithm's estimate of the gyroscope bias temperature - * coefficient. - * The argument array elements are ordered X,Y,Z. - * Values are in units of dps per deg C (degrees per second per degree - * Celcius) - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long </b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_gyro_temp_slope_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) { - data[0] = inv_obj.x_gyro_coef[1]; - data[1] = inv_obj.y_gyro_coef[1]; - data[2] = inv_obj.z_gyro_coef[1]; - } else { - data[0] = (float)inv_obj.temp_slope[0] / 65536.0f; - data[1] = (float)inv_obj.temp_slope[1] / 65536.0f; - data[2] = (float)inv_obj.temp_slope[2] / 65536.0f; - } - - return result; -} - -/** - * @brief inv_get_gyro_bias_float is used to get the gyroscope biases. - * The argument array elements are ordered X,Y,Z. - * The values are in units of dps (degrees per second). - - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_gyro_bias_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.gyro_bias[0] / 65536.0f; - data[1] = (float)inv_obj.gyro_bias[1] / 65536.0f; - data[2] = (float)inv_obj.gyro_bias[2] / 65536.0f; - - return result; -} - -/** - * @brief inv_get_accel_bias_float is used to get the accelerometer baises. - * The argument array elements are ordered X,Y,Z. - * The values are in units of g (gravity). - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_accel_bias_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.accel_bias[0] / 65536.0f; - data[1] = (float)inv_obj.accel_bias[1] / 65536.0f; - data[2] = (float)inv_obj.accel_bias[2] / 65536.0f; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_bias_float is used to get an array of three data points representing - * the compass biases. - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_bias_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = - ((float) - (inv_obj.compass_bias[0] + - (long)((long long)inv_obj.init_compass_bias[0] * - inv_obj.compass_sens / 16384))) / 65536.0f; - data[1] = - ((float) - (inv_obj.compass_bias[1] + - (long)((long long)inv_obj.init_compass_bias[1] * - inv_obj.compass_sens / 16384))) / 65536.0f; - data[2] = - ((float) - (inv_obj.compass_bias[2] + - (long)((long long)inv_obj.init_compass_bias[2] * - inv_obj.compass_sens / 16384))) / 65536.0f; - - return result; -} - -/** - * @brief inv_get_gyro_and_accel_sensor_float is used to get the most recent set of all sensor data. - * The argument array elements are ordered gyroscope X,Y, and Z, - * accelerometer X, Y, and Z, and magnetometer X,Y, and Z. - * \if UMPL The magnetometer elements are not populated in UMPL. \endif - * The gyroscope and accelerometer data is not scaled or offset, it is - * copied directly from the sensor registers, and cast as a float. - * In the case of accelerometers with 8-bit output resolution, the data - * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g - * full scale range - - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_gyro_and_accel_sensor_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - long ldata[6]; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_gyro_and_accel_sensor(ldata); - data[0] = (float)ldata[0]; - data[1] = (float)ldata[1]; - data[2] = (float)ldata[2]; - data[3] = (float)ldata[3]; - data[4] = (float)ldata[4]; - data[5] = (float)ldata[5]; - data[6] = (float)inv_obj.compass_sensor_data[0]; - data[7] = (float)inv_obj.compass_sensor_data[1]; - data[8] = (float)inv_obj.compass_sensor_data[2]; - - return result; -} - -/** - * @brief inv_get_euler_angles_x is used to get the Euler angle representation - * of the current sensor fusion solution. - * Euler angles are returned according to the X convention. - * This is typically the convention used for mobile devices where the X - * axis is the width of the screen, Y axis is the height, and Z the - * depth. In this case roll is defined as the rotation around the X - * axis of the device. - * <TABLE> - * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR> - * <TR><TD> 0 </TD><TD>Roll </TD><TD>X axis </TD></TR> - * <TR><TD> 1 </TD><TD>Pitch </TD><TD>Y axis </TD></TR> - * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR> - * - </TABLE> - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_euler_angles_x_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_rot_mat_float(rotMatrix); - tmp = rotMatrix[6]; - if (tmp > 1.0f) { - tmp = 1.0f; - } - if (tmp < -1.0f) { - tmp = -1.0f; - } - data[0] = - (float)(atan2f(rotMatrix[7], - rotMatrix[8]) * 57.29577951308); - data[1] = (float)((double)asin(tmp) * 57.29577951308); - data[2] = - (float)(atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308); - - return result; -} - -/** - * @brief inv_get_euler_angles_float is used to get an array of three data points three data points - * representing roll, pitch, and yaw corresponding to the INV_EULER_ANGLES_X output and it is - * therefore the default convention for Euler angles. - * Please refer to the INV_EULER_ANGLES_X for a detailed description. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_euler_angles_float(float *data) -{ - return inv_get_euler_angles_x_float(data); -} - -/** @brief inv_get_euler_angles_y_float is used to get the Euler angle representation - * of the current sensor fusion solution. - * Euler angles are returned according to the Y convention. - * This convention is typically used in augmented reality applications, - * where roll is defined as the rotation around the axis along the - * height of the screen of a mobile device, namely the Y axis. - * <TABLE> - * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR> - * <TR><TD> 0 </TD><TD>Roll </TD><TD>Y axis </TD></TR> - * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR> - * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_euler_angles_y_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_rot_mat_float(rotMatrix); - tmp = rotMatrix[7]; - if (tmp > 1.0f) { - tmp = 1.0f; - } - if (tmp < -1.0f) { - tmp = -1.0f; - } - data[0] = - (float)(atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308); - data[1] = (float)((double)asin(tmp) * 57.29577951308); - data[2] = - (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308); - - return result; -} - -/** @brief inv_get_euler_angles_z_float is used to get the Euler angle representation - * of the current sensor fusion solution. - * This convention is mostly used in application involving the use - * of a camera, typically placed on the back of a mobile device, that - * is along the Z axis. In this convention roll is defined as the - * rotation around the Z axis. - * Euler angles are returned according to the Y convention. - * <TABLE> - * <TR><TD>Element </TD><TD><b>Euler angle</b></TD><TD><b>Rotation about </b></TD></TR> - * <TR><TD> 0 </TD><TD>Roll </TD><TD>Z axis </TD></TR> - * <TR><TD> 1 </TD><TD>Pitch </TD><TD>X axis </TD></TR> - * <TR><TD> 2 </TD><TD>Yaw </TD><TD>Y axis </TD></TR> - * </TABLE> - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_euler_angles_z_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - result = inv_get_rot_mat_float(rotMatrix); - tmp = rotMatrix[8]; - if (tmp > 1.0f) { - tmp = 1.0f; - } - if (tmp < -1.0f) { - tmp = -1.0f; - } - data[0] = - (float)(atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308); - data[1] = (float)((double)asin(tmp) * 57.29577951308); - data[2] = - (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308); - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_raw_data_float is used to get Raw magnetometer data - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_raw_data_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = - (float)(inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]); - data[1] = - (float)(inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]); - data[2] = - (float)(inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]); - - return result; -} - -/** - * @cond MPL - * @brief inv_get_magnetometer_float is used to get magnetometer data - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_magnetometer_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.compass_calibrated_data[0] / 65536.0f; - data[1] = (float)inv_obj.compass_calibrated_data[1] / 65536.0f; - data[2] = (float)inv_obj.compass_calibrated_data[2] / 65536.0f; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_pressure_float is used to get a single value representing the pressure in Pascal - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to the data to be passed back to the user. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_pressure_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.pressure; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_heading_float is used to get single number representing the heading of the device - * relative to the Earth, in which 0 represents North, 90 degrees - * represents East, and so on. - * The heading is defined as the direction of the +Y axis if the Y - * axis is horizontal, and otherwise the direction of the -Z axis. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to the data to be passed back to the user. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_heading_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - float rotMatrix[9]; - float tmp; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - inv_get_rot_mat_float(rotMatrix); - if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) { - tmp = - (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 - - 90.0f); - } else { - tmp = - (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 + - 90.0f); - } - if (tmp < 0) { - tmp += 360.0f; - } - data[0] = 360 - tmp; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_bias_error_float is used to get an array of three numbers representing - * the current estimated error in the compass biases. These numbers are unitless and serve - * as rough estimates in which numbers less than 100 typically represent - * reasonably well calibrated compass axes. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_bias_error_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (inv_obj.large_field == 0) { - data[0] = (float)inv_obj.compass_bias_error[0]; - data[1] = (float)inv_obj.compass_bias_error[1]; - data[2] = (float)inv_obj.compass_bias_error[2]; - } else { - data[0] = (float)P_INIT; - data[1] = (float)P_INIT; - data[2] = (float)P_INIT; - } - - return result; -} - -/** - * @cond MPL - * @brief inv_get_mag_scale_float is used to get magnetometer scale. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_mag_scale_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.compass_scale[0] / 65536.0f; - data[1] = (float)inv_obj.compass_scale[1] / 65536.0f; - data[2] = (float)inv_obj.compass_scale[2] / 65536.0f; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_local_field_float is used to get local magnetic field data. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 3 cells long</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_local_field_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.local_field[0] / 65536.0f; - data[1] = (float)inv_obj.local_field[1] / 65536.0f; - data[2] = (float)inv_obj.local_field[2] / 65536.0f; - - return result; -} - -/** - * @cond MPL - * @brief inv_get_relative_quaternion_float is used to get relative quaternion data. - * - * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif - * must have been called. - * - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 4 cells long at least</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - * @endcond - */ -inv_error_t inv_get_relative_quaternion_float(float *data) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - data[0] = (float)inv_obj.relative_quat[0] / 1073741824.0f; - data[1] = (float)inv_obj.relative_quat[1] / 1073741824.0f; - data[2] = (float)inv_obj.relative_quat[2] / 1073741824.0f; - data[3] = (float)inv_obj.relative_quat[3] / 1073741824.0f; - - return result; -} - -/** - * Returns the curren compass accuracy. - * - * - 0: Unknown: The accuracy is unreliable and compass data should not be used - * - 1: Low: The compass accuracy is low. - * - 2: Medium: The compass accuracy is medium. - * - 3: High: The compas acurracy is high and can be trusted - * - * @param accuracy The accuracy level in the range 0-3 - * - * @return ML_SUCCESS or non-zero error code - */ -inv_error_t inv_get_compass_accuracy(int *accuracy) -{ - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - *accuracy = inv_obj.compass_accuracy; - return INV_SUCCESS; -} - -/** - * @brief inv_set_gyro_bias is used to set the gyroscope bias. - * The argument array elements are ordered X,Y,Z. - * The values are scaled at 1 dps = 2^16 LSBs. - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_gyro_bias(long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - long biasTmp; - long sf = 0; - short offset[GYRO_NUM_AXES]; - int i; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - if (mldl_cfg->gyro_sens_trim != 0) { - sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; - } else { - sf = 2000; - } - for (i = 0; i < GYRO_NUM_AXES; i++) { - inv_obj.gyro_bias[i] = data[i]; - biasTmp = -inv_obj.gyro_bias[i] / sf; - if (biasTmp < 0) - biasTmp += 65536L; - offset[i] = (short)biasTmp; - } - result = inv_set_offset(offset); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return INV_SUCCESS; -} - -/** - * @brief inv_set_accel_bias is used to set the accelerometer bias. - * The argument array elements are ordered X,Y,Z. - * The values are scaled in units of g (gravity), - * where 1 g = 2^16 LSBs. - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_accel_bias(long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - long biasTmp; - int i, j; - unsigned char regs[6]; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - for (i = 0; i < ACCEL_NUM_AXES; i++) { - inv_obj.accel_bias[i] = data[i]; - if (inv_obj.accel_sens != 0 && mldl_cfg && mldl_cfg->pdata) { - long long tmp64; - inv_obj.scaled_accel_bias[i] = 0; - for (j = 0; j < ACCEL_NUM_AXES; j++) { - inv_obj.scaled_accel_bias[i] += - data[j] * - (long)mldl_cfg->pdata->accel.orientation[i * 3 + j]; - } - tmp64 = (long long)inv_obj.scaled_accel_bias[i] << 13; - biasTmp = (long)(tmp64 / inv_obj.accel_sens); - } else { - biasTmp = 0; - } - if (biasTmp < 0) - biasTmp += 65536L; - regs[2 * i + 0] = (unsigned char)(biasTmp / 256); - regs[2 * i + 1] = (unsigned char)(biasTmp % 256); - } - result = inv_set_mpu_memory(KEY_D_1_8, 2, ®s[0]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_mpu_memory(KEY_D_1_10, 2, ®s[2]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_set_mpu_memory(KEY_D_1_2, 2, ®s[4]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return INV_SUCCESS; -} - -/** - * @cond MPL - * @brief inv_set_mag_bias is used to set Compass Bias - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - * @endcond - */ -inv_error_t inv_set_mag_bias(long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - inv_set_compass_bias(data); - inv_obj.init_compass_bias[0] = 0; - inv_obj.init_compass_bias[1] = 0; - inv_obj.init_compass_bias[2] = 0; - inv_obj.got_compass_bias = 1; - inv_obj.got_init_compass_bias = 1; - inv_obj.compass_state = SF_STARTUP_SETTLE; - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return INV_SUCCESS; -} - -/** - * @brief inv_set_gyro_temp_slope is used to set the temperature - * compensation algorithm's estimate of the gyroscope bias temperature - * coefficient. - * The argument array elements are ordered X,Y,Z. - * Values are in units of dps per deg C (degrees per second per degree - * Celcius), and scaled such that 1 dps per deg C = 2^16 LSBs. - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document. - * - * @brief inv_set_gyro_temp_slope is used to set Gyro temperature slope - * - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_gyro_temp_slope(long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - int i; - long sf; - unsigned char regs[3]; - - inv_obj.factory_temp_comp = 1; - inv_obj.temp_slope[0] = data[0]; - inv_obj.temp_slope[1] = data[1]; - inv_obj.temp_slope[2] = data[2]; - for (i = 0; i < GYRO_NUM_AXES; i++) { - sf = -inv_obj.temp_slope[i] / 1118; - if (sf > 127) { - sf -= 256; - } - regs[i] = (unsigned char)sf; - } - result = inv_set_offsetTC(regs); - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return INV_SUCCESS; -} - -/** - * @cond MPL - * @brief inv_set_local_field is used to set local magnetic field - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - * @endcond - */ -inv_error_t inv_set_local_field(long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - inv_obj.local_field[0] = data[0]; - inv_obj.local_field[1] = data[1]; - inv_obj.local_field[2] = data[2]; - inv_obj.new_local_field = 1; - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return INV_SUCCESS; -} - -/** - * @cond MPL - * @brief inv_set_mag_scale is used to set magnetometer scale - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - * @endcond - */ -inv_error_t inv_set_mag_scale(long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - inv_obj.compass_scale[0] = data[0]; - inv_obj.compass_scale[1] = data[1]; - inv_obj.compass_scale[2] = data[2]; - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return INV_SUCCESS; -} - -/** - * @brief inv_set_gyro_temp_slope_float is used to get the temperature - * compensation algorithm's estimate of the gyroscope bias temperature - * coefficient. - * The argument array elements are ordered X,Y,Z. - * Values are in units of dps per deg C (degrees per second per degree - * Celcius) - - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_gyro_temp_slope_float(float *data) -{ - long arrayTmp[3]; - arrayTmp[0] = (long)(data[0] * 65536.f); - arrayTmp[1] = (long)(data[1] * 65536.f); - arrayTmp[2] = (long)(data[2] * 65536.f); - return inv_set_gyro_temp_slope(arrayTmp); -} - -/** - * @brief inv_set_gyro_bias_float is used to set the gyroscope bias. - * The argument array elements are ordered X,Y,Z. - * The values are in units of dps (degrees per second). - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_gyro_bias_float(float *data) -{ - long arrayTmp[3]; - arrayTmp[0] = (long)(data[0] * 65536.f); - arrayTmp[1] = (long)(data[1] * 65536.f); - arrayTmp[2] = (long)(data[2] * 65536.f); - return inv_set_gyro_bias(arrayTmp); - -} - -/** - * @brief inv_set_accel_bias_float is used to set the accelerometer bias. - * The argument array elements are ordered X,Y,Z. - * The values are in units of g (gravity). - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_accel_bias_float(float *data) -{ - long arrayTmp[3]; - arrayTmp[0] = (long)(data[0] * 65536.f); - arrayTmp[1] = (long)(data[1] * 65536.f); - arrayTmp[2] = (long)(data[2] * 65536.f); - return inv_set_accel_bias(arrayTmp); - -} - -/** - * @cond MPL - * @brief inv_set_mag_bias_float is used to set compass bias - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen()\ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - * @endcond - */ -inv_error_t inv_set_mag_bias_float(float *data) -{ - long arrayTmp[3]; - arrayTmp[0] = (long)(data[0] * 65536.f); - arrayTmp[1] = (long)(data[1] * 65536.f); - arrayTmp[2] = (long)(data[2] * 65536.f); - return inv_set_mag_bias(arrayTmp); -} - -/** - * @cond MPL - * @brief inv_set_local_field_float is used to set local magnetic field - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - * @endcond - */ -inv_error_t inv_set_local_field_float(float *data) -{ - long arrayTmp[3]; - arrayTmp[0] = (long)(data[0] * 65536.f); - arrayTmp[1] = (long)(data[1] * 65536.f); - arrayTmp[2] = (long)(data[2] * 65536.f); - return inv_set_local_field(arrayTmp); -} - -/** - * @cond MPL - * @brief inv_set_mag_scale_float is used to set magnetometer scale - * - * Please refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() \ifnot UMPL or - * MLDmpPedometerStandAloneOpen() \endif - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - * @endcond - */ -inv_error_t inv_set_mag_scale_float(float *data) -{ - long arrayTmp[3]; - arrayTmp[0] = (long)(data[0] * 65536.f); - arrayTmp[1] = (long)(data[1] * 65536.f); - arrayTmp[2] = (long)(data[2] * 65536.f); - return inv_set_mag_scale(arrayTmp); -} diff --git a/60xx/mlsdk/mllite/mlarray_legacy.c b/60xx/mlsdk/mllite/mlarray_legacy.c deleted file mode 100644 index 9dce8f3..0000000 --- a/60xx/mlsdk/mllite/mlarray_legacy.c +++ /dev/null @@ -1,587 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: mlarray_legacy.c $ - * - *****************************************************************************/ - -/** - * @defgroup MLArray_Legacy - * @brief Legacy Motion Library Array APIs. - * The Motion Library Array APIs provide the user access to the - * Motion Library state. These Legacy APIs provide access to - * individual state arrays using a data set name as the first - * argument to the API. This format has been replaced by unique - * named APIs for each data set, found in the MLArray group. - * - * @{ - * @file mlarray_legacy.c - * @brief The Legacy Motion Library Array APIs. - */ - -#include "ml.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "mlFIFO.h" -#include "mldl_cfg.h" - -/** - * @brief inv_get_array is used to get an array of processed motion sensor data. - * inv_get_array can be used to retrieve various data sets. Certain data - * sets require functions to be enabled using MLEnable in order to be - * valid. - * - * The available data sets are: - * - * - INV_ROTATION_MATRIX - * - INV_QUATERNION - * - INV_EULER_ANGLES_X - * - INV_EULER_ANGLES_Y - * - INV_EULER_ANGLES_Z - * - INV_EULER_ANGLES - * - INV_LINEAR_ACCELERATION - * - INV_LINEAR_ACCELERATION_WORLD - * - INV_GRAVITY - * - INV_ANGULAR_VELOCITY - * - INV_RAW_DATA - * - INV_GYROS - * - INV_ACCELS - * - INV_MAGNETOMETER - * - INV_GYRO_BIAS - * - INV_ACCEL_BIAS - * - INV_MAG_BIAS - * - INV_HEADING - * - INV_MAG_BIAS_ERROR - * - INV_PRESSURE - * - * Please refer to the documentation of inv_get_float_array() for a - * description of these data sets. - * - * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() - * must have been called. - * - * @param dataSet - * A constant specifying an array of data processed by the - * motion processor. - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_array(int dataSet, long *data) -{ - inv_error_t result; - switch (dataSet) { - case INV_GYROS: - result = inv_get_gyro(data); - break; - case INV_ACCELS: - result = inv_get_accel(data); - break; - case INV_TEMPERATURE: - result = inv_get_temperature(data); - break; - case INV_ROTATION_MATRIX: - result = inv_get_rot_mat(data); - break; - case INV_QUATERNION: - result = inv_get_quaternion(data); - break; - case INV_LINEAR_ACCELERATION: - result = inv_get_linear_accel(data); - break; - case INV_LINEAR_ACCELERATION_WORLD: - result = inv_get_linear_accel_in_world(data); - break; - case INV_GRAVITY: - result = inv_get_gravity(data); - break; - case INV_ANGULAR_VELOCITY: - result = inv_get_angular_velocity(data); - break; - case INV_EULER_ANGLES: - result = inv_get_euler_angles(data); - break; - case INV_EULER_ANGLES_X: - result = inv_get_euler_angles_x(data); - break; - case INV_EULER_ANGLES_Y: - result = inv_get_euler_angles_y(data); - break; - case INV_EULER_ANGLES_Z: - result = inv_get_euler_angles_z(data); - break; - case INV_GYRO_TEMP_SLOPE: - result = inv_get_gyro_temp_slope(data); - break; - case INV_GYRO_BIAS: - result = inv_get_gyro_bias(data); - break; - case INV_ACCEL_BIAS: - result = inv_get_accel_bias(data); - break; - case INV_MAG_BIAS: - result = inv_get_mag_bias(data); - break; - case INV_RAW_DATA: - result = inv_get_gyro_and_accel_sensor(data); - break; - case INV_MAG_RAW_DATA: - result = inv_get_mag_raw_data(data); - break; - case INV_MAGNETOMETER: - result = inv_get_magnetometer(data); - break; - case INV_PRESSURE: - result = inv_get_pressure(data); - break; - case INV_HEADING: - result = inv_get_heading(data); - break; - case INV_GYRO_CALIBRATION_MATRIX: - result = inv_get_gyro_cal_matrix(data); - break; - case INV_ACCEL_CALIBRATION_MATRIX: - result = inv_get_accel_cal_matrix(data); - break; - case INV_MAG_CALIBRATION_MATRIX: - result = inv_get_mag_cal_matrix(data); - break; - case INV_MAG_BIAS_ERROR: - result = inv_get_mag_bias_error(data); - break; - case INV_MAG_SCALE: - result = inv_get_mag_scale(data); - break; - case INV_LOCAL_FIELD: - result = inv_get_local_field(data); - break; - case INV_RELATIVE_QUATERNION: - result = inv_get_relative_quaternion(data); - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief inv_get_float_array is used to get an array of processed motion sensor - * data. inv_get_array can be used to retrieve various data sets. - * Certain data sets require functions to be enabled using MLEnable - * in order to be valid. - * - * The available data sets are: - * - * - INV_ROTATION_MATRIX : - * Returns an array of nine data points representing the rotation - * matrix generated from all available sensors. - * This requires that ML_SENSOR_FUSION be enabled. - * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, - * R33, representing the matrix: - * <center>R11 R12 R13</center> - * <center>R21 R22 R23</center> - * <center>R31 R32 R33</center> - * <b>Please refer to the "9-Axis Sensor Fusion Application Note" document, - * section 7 "Sensor Fusion Output", for details regarding rotation - * matrix output</b>. - * - * - INV_QUATERNION : - * Returns an array of four data points representing the quaternion - * generated from all available sensors. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_EULER_ANGLES_X : - * Returns an array of three data points representing roll, pitch, and - * yaw using the X axis of the gyroscope, accelerometer, and compass as - * reference axis. - * This is typically the convention used for mobile devices where the X - * axis is the width of the screen, Y axis is the height, and Z the - * depth. In this case roll is defined as the rotation around the X - * axis of the device. - * The euler angles convention for this output is the following: - * <TABLE> - * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> - * <TR><TD>roll </TD><TD>X axis </TD></TR> - * <TR><TD>pitch </TD><TD>Y axis </TD></TR> - * <TR><TD>yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * INV_EULER_ANGLES_X corresponds to the INV_EULER_ANGLES output and is - * therefore the default convention. - * - * - INV_EULER_ANGLES_Y : - * Returns an array of three data points representing roll, pitch, and - * yaw using the Y axis of the gyroscope, accelerometer, and compass as - * reference axis. - * This convention is typically used in augmented reality applications, - * where roll is defined as the rotation around the axis along the - * height of the screen of a mobile device, namely the Y axis. - * The euler angles convention for this output is the following: - * <TABLE> - * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> - * <TR><TD>roll </TD><TD>Y axis </TD></TR> - * <TR><TD>pitch </TD><TD>X axis </TD></TR> - * <TR><TD>yaw </TD><TD>Z axis </TD></TR> - * </TABLE> - * - * - INV_EULER_ANGLES_Z : - * Returns an array of three data points representing roll, pitch, and - * yaw using the Z axis of the gyroscope, accelerometer, and compass as - * reference axis. - * This convention is mostly used in application involving the use - * of a camera, typically placed on the back of a mobile device, that - * is along the Z axis. In this convention roll is defined as the - * rotation around the Z axis. - * The euler angles convention for this output is the following: - * <TABLE> - * <TR><TD><b>EULER ANGLE</b></TD><TD><b>ROTATION AROUND</b></TD></TR> - * <TR><TD>roll </TD><TD>Z axis </TD></TR> - * <TR><TD>pitch </TD><TD>X axis </TD></TR> - * <TR><TD>yaw </TD><TD>Y axis </TD></TR> - * </TABLE> - * - * - INV_EULER_ANGLES : - * Returns an array of three data points representing roll, pitch, and - * yaw corresponding to the INV_EULER_ANGLES_X output and it is - * therefore the default convention for Euler angles. - * Please refer to the INV_EULER_ANGLES_X for a detailed description. - * - * - INV_LINEAR_ACCELERATION : - * Returns an array of three data points representing the linear - * acceleration as derived from both gyroscopes and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_LINEAR_ACCELERATION_WORLD : - * Returns an array of three data points representing the linear - * acceleration in world coordinates, as derived from both gyroscopes - * and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_GRAVITY : - * Returns an array of three data points representing the direction - * of gravity in body coordinates, as derived from both gyroscopes - * and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled. - * - * - INV_ANGULAR_VELOCITY : - * Returns an array of three data points representing the angular - * velocity as derived from <b>both</b> gyroscopes and accelerometers. - * This requires that ML_SENSOR_FUSION be enabled, to fuse data from - * the gyroscope and accelerometer device, appropriately scaled and - * oriented according to the respective mounting matrices. - * - * - INV_RAW_DATA : - * Returns an array of nine data points representing raw sensor data - * of the gyroscope X, Y, Z, accelerometer X, Y, Z, and - * compass X, Y, Z values. - * These values are not scaled and come out directly from the devices' - * sensor data output. In case of accelerometers with lower output - * resolution, e.g 8-bit, the sensor data is scaled up to match the - * 2^14 = 1 gee typical representation for a +/- 2 gee full scale - * range. - * - * - INV_GYROS : - * Returns an array of three data points representing the X gyroscope, - * Y gyroscope, and Z gyroscope values. - * The values are not sensor fused with other sensor types data but - * reflect the orientation from the mounting matrices in use. - * The INV_GYROS values are scaled to ensure 1 dps corresponds to 2^16 - * codes. - * - * - INV_ACCELS : - * Returns an array of three data points representing the X - * accelerometer, Y accelerometer, and Z accelerometer values. - * The values are not sensor fused with other sensor types data but - * reflect the orientation from the mounting matrices in use. - * The INV_ACCELS values are scaled to ensure 1 gee corresponds to 2^16 - * codes. - * - * - INV_MAGNETOMETER : - * Returns an array of three data points representing the compass - * X, Y, and Z values. - * The values are not sensor fused with other sensor types data but - * reflect the orientation from the mounting matrices in use. - * The INV_MAGNETOMETER values are scaled to ensure 1 micro Tesla (uT) - * corresponds to 2^16 codes. - * - * - INV_GYRO_BIAS : - * Returns an array of three data points representing the gyroscope - * biases. - * - * - INV_ACCEL_BIAS : - * Returns an array of three data points representing the - * accelerometer biases. - * - * - INV_MAG_BIAS : - * Returns an array of three data points representing the compass - * biases. - * - * - INV_GYRO_CALIBRATION_MATRIX : - * Returns an array of nine data points representing the calibration - * matrix for the gyroscopes: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * - INV_ACCEL_CALIBRATION_MATRIX : - * Returns an array of nine data points representing the calibration - * matrix for the accelerometers: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * - INV_MAG_CALIBRATION_MATRIX : - * Returns an array of nine data points representing the calibration - * matrix for the compass: - * <center>C11 C12 C13</center> - * <center>C21 C22 C23</center> - * <center>C31 C32 C33</center> - * - * - INV_PRESSURE : - * Returns a single value representing the pressure in Pascal - * - * - INV_HEADING : - * Returns a single number representing the heading of the device - * relative to the Earth, in which 0 represents North, 90 degrees - * represents East, and so on. - * The heading is defined as the direction of the +Y axis if the Y - * axis is horizontal, and otherwise the direction of the -Z axis. - * - * - INV_MAG_BIAS_ERROR : - * Returns an array of three numbers representing the current estimated - * error in the compass biases. These numbers are unitless and serve - * as rough estimates in which numbers less than 100 typically represent - * reasonably well calibrated compass axes. - * - * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() - * must have been called. - * - * @param dataSet - * A constant specifying an array of data processed by - * the motion processor. - * @param data - * A pointer to an array to be passed back to the user. - * <b>Must be 9 cells long at least</b>. - * - * @return INV_SUCCESS if the command is successful; an error code otherwise. - */ -inv_error_t inv_get_float_array(int dataSet, float *data) -{ - inv_error_t result; - switch (dataSet) { - case INV_GYROS: - result = inv_get_gyro_float(data); - break; - case INV_ACCELS: - result = inv_get_accel_float(data); - break; - case INV_TEMPERATURE: - result = inv_get_temperature_float(data); - break; - case INV_ROTATION_MATRIX: - result = inv_get_rot_mat_float(data); - break; - case INV_QUATERNION: - result = inv_get_quaternion_float(data); - break; - case INV_LINEAR_ACCELERATION: - result = inv_get_linear_accel_float(data); - break; - case INV_LINEAR_ACCELERATION_WORLD: - result = inv_get_linear_accel_in_world_float(data); - break; - case INV_GRAVITY: - result = inv_get_gravity_float(data); - break; - case INV_ANGULAR_VELOCITY: - result = inv_get_angular_velocity_float(data); - break; - case INV_EULER_ANGLES: - result = inv_get_euler_angles_float(data); - break; - case INV_EULER_ANGLES_X: - result = inv_get_euler_angles_x_float(data); - break; - case INV_EULER_ANGLES_Y: - result = inv_get_euler_angles_y_float(data); - break; - case INV_EULER_ANGLES_Z: - result = inv_get_euler_angles_z_float(data); - break; - case INV_GYRO_TEMP_SLOPE: - result = inv_get_gyro_temp_slope_float(data); - break; - case INV_GYRO_BIAS: - result = inv_get_gyro_bias_float(data); - break; - case INV_ACCEL_BIAS: - result = inv_get_accel_bias_float(data); - break; - case INV_MAG_BIAS: - result = inv_get_mag_bias_float(data); - break; - case INV_RAW_DATA: - result = inv_get_gyro_and_accel_sensor_float(data); - break; - case INV_MAG_RAW_DATA: - result = inv_get_mag_raw_data_float(data); - break; - case INV_MAGNETOMETER: - result = inv_get_magnetometer_float(data); - break; - case INV_PRESSURE: - result = inv_get_pressure_float(data); - break; - case INV_HEADING: - result = inv_get_heading_float(data); - break; - case INV_GYRO_CALIBRATION_MATRIX: - result = inv_get_gyro_cal_matrix_float(data); - break; - case INV_ACCEL_CALIBRATION_MATRIX: - result = inv_get_accel_cal_matrix_float(data); - break; - case INV_MAG_CALIBRATION_MATRIX: - result = inv_get_mag_cal_matrix_float(data); - break; - case INV_MAG_BIAS_ERROR: - result = inv_get_mag_bias_error_float(data); - break; - case INV_MAG_SCALE: - result = inv_get_mag_scale_float(data); - break; - case INV_LOCAL_FIELD: - result = inv_get_local_field_float(data); - break; - case INV_RELATIVE_QUATERNION: - result = inv_get_relative_quaternion_float(data); - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief used to set an array of motion sensor data. - * Handles the following data sets: - * - INV_GYRO_BIAS - * - INV_ACCEL_BIAS - * - INV_MAG_BIAS - * - INV_GYRO_TEMP_SLOPE - * - * For more details about the use of the data sets - * please refer to the documentation of inv_set_float_array(). - * - * Please also refer to the provided "9-Axis Sensor Fusion - * Application Note" document provided. - * - * @pre MLDmpOpen() or - * MLDmpPedometerStandAloneOpen() - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param dataSet A constant specifying an array of data. - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_array(int dataSet, long *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - switch (dataSet) { - case INV_GYRO_BIAS: - result = inv_set_gyro_bias(data); - break; - case INV_ACCEL_BIAS: - result = inv_set_accel_bias(data); - break; - case INV_MAG_BIAS: - result = inv_set_mag_bias(data); - break; - case INV_GYRO_TEMP_SLOPE: - result = inv_set_gyro_temp_slope(data); - break; - case INV_LOCAL_FIELD: - result = inv_set_local_field(data); - break; - case INV_MAG_SCALE: - result = inv_set_mag_scale(data); - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief used to set an array of motion sensor data. - * Handles various data sets: - * - INV_GYRO_BIAS - * - INV_ACCEL_BIAS - * - INV_MAG_BIAS - * - INV_GYRO_TEMP_SLOPE - * - * Please refer to the provided "9-Axis Sensor Fusion Application - * Note" document provided. - * - * @pre MLDmpOpen() or - * MLDmpPedometerStandAloneOpen() - * @pre MLDmpStart() must <b>NOT</b> have been called. - * - * @param dataSet A constant specifying an array of data. - * @param data A pointer to an array to be copied from the user. - * - * @return INV_SUCCESS if successful; a non-zero error code otherwise. - */ -inv_error_t inv_set_float_array(int dataSet, float *data) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - switch (dataSet) { - case INV_GYRO_TEMP_SLOPE: // internal - result = inv_set_gyro_temp_slope_float(data); - break; - case INV_GYRO_BIAS: // internal - result = inv_set_gyro_bias_float(data); - break; - case INV_ACCEL_BIAS: // internal - result = inv_set_accel_bias_float(data); - break; - case INV_MAG_BIAS: // internal - result = inv_set_mag_bias_float(data); - break; - case INV_LOCAL_FIELD: // internal - result = inv_set_local_field_float(data); - break; - case INV_MAG_SCALE: // internal - result = inv_set_mag_scale_float(data); - break; - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} diff --git a/60xx/mlsdk/mllite/mlcontrol.c b/60xx/mlsdk/mllite/mlcontrol.c deleted file mode 100644 index bd186fa..0000000 --- a/60xx/mlsdk/mllite/mlcontrol.c +++ /dev/null @@ -1,797 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/******************************************************************************* - * - * $Id: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup CONTROL - * @brief Motion Library - Control Engine. - * The Control Library processes gyroscopes, accelerometers, and - * compasses to provide control signals that can be used in user - * interfaces. - * These signals can be used to manipulate objects such as documents, - * images, cursors, menus, etc. - * - * @{ - * @file mlcontrol.c - * @brief The Control Library. - * - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include "mltypes.h" -#include "mlinclude.h" -#include "mltypes.h" -#include "ml.h" -#include "mlos.h" -#include "mlsl.h" -#include "mldl.h" -#include "mlcontrol.h" -#include "dmpKey.h" -#include "mlstates.h" -#include "mlFIFO.h" -#include "string.h" - -/* - Global Vars. - */ -struct control_params cntrl_params = { - { - MLCTRL_SENSITIVITY_0_DEFAULT, - MLCTRL_SENSITIVITY_1_DEFAULT, - MLCTRL_SENSITIVITY_2_DEFAULT, - MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity - MLCTRL_FUNCTIONS_DEFAULT, // functions - { - MLCTRL_PARAMETER_ARRAY_0_DEFAULT, - MLCTRL_PARAMETER_ARRAY_1_DEFAULT, - MLCTRL_PARAMETER_ARRAY_2_DEFAULT, - MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray - { - MLCTRL_PARAMETER_AXIS_0_DEFAULT, - MLCTRL_PARAMETER_AXIS_1_DEFAULT, - MLCTRL_PARAMETER_AXIS_2_DEFAULT, - MLCTRL_PARAMETER_AXIS_3_DEFAULT}, // parameterAxis - { - MLCTRL_GRID_THRESHOLD_0_DEFAULT, - MLCTRL_GRID_THRESHOLD_1_DEFAULT, - MLCTRL_GRID_THRESHOLD_2_DEFAULT, - MLCTRL_GRID_THRESHOLD_3_DEFAULT}, // gridThreshold - { - MLCTRL_GRID_MAXIMUM_0_DEFAULT, - MLCTRL_GRID_MAXIMUM_1_DEFAULT, - MLCTRL_GRID_MAXIMUM_2_DEFAULT, - MLCTRL_GRID_MAXIMUM_3_DEFAULT}, // gridMaximum - MLCTRL_GRID_CALLBACK_DEFAULT // gridCallback -}; - -/* - Extern Vars. - */ -struct control_obj cntrl_obj; -extern const unsigned char *dmpConfig1; - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief inv_set_control_sensitivity is used to set the sensitivity for a control - * signal. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param sensitivity The sensitivity of the control signal. - * - * @return error code - */ -inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, - long sensitivity) -{ - INVENSENSE_FUNC_START; - unsigned char regs[2]; - long finalSens = 0; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - finalSens = sensitivity * 100; - if (finalSens > 16384) { - finalSens = 16384; - } - regs[0] = (unsigned char)(finalSens / 256); - regs[1] = (unsigned char)(finalSens % 256); - switch (controlSignal) { - case INV_CONTROL_1: - result = inv_set_mpu_memory(KEY_D_0_224, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[0] = (unsigned short)sensitivity; - break; - case INV_CONTROL_2: - result = inv_set_mpu_memory(KEY_D_0_228, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[1] = (unsigned short)sensitivity; - break; - case INV_CONTROL_3: - result = inv_set_mpu_memory(KEY_D_0_232, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[2] = (unsigned short)sensitivity; - break; - case INV_CONTROL_4: - result = inv_set_mpu_memory(KEY_D_0_236, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.sensitivity[3] = (unsigned short)sensitivity; - break; - default: - break; - } - if (finalSens != sensitivity * 100) { - return INV_ERROR_INVALID_PARAMETER; - } else { - return INV_SUCCESS; - } -} - -/** - * @brief inv_set_control_func allows the user to choose how the sensor data will - * be processed in order to provide a control parameter. - * inv_set_control_func allows the user to choose which control functions - * will be incorporated in the sensor data processing. - * The control functions are: - * - INV_GRID - * Indicates that the user will be controlling a system that - * has discrete steps, such as icons, menu entries, pixels, etc. - * - INV_SMOOTH - * Indicates that noise from unintentional motion should be filtered out. - * - INV_DEAD_ZONE - * Indicates that a dead zone should be used, below which sensor - * data is set to zero. - * - INV_HYSTERESIS - * Indicates that, when INV_GRID is selected, hysteresis should - * be used to prevent the control signal from switching rapidly across - * elements of the grid. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param function Indicates what functions will be used. - * Can be a bitwise OR of several values. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_set_control_func(unsigned short function) -{ - INVENSENSE_FUNC_START; - unsigned char regs[8] = { DINA06, DINA26, - DINA46, DINA66, - DINA0E, DINA2E, - DINA4E, DINA6E - }; - unsigned char i; - inv_error_t result; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if ((function & INV_SMOOTH) == 0) { - for (i = 0; i < 8; i++) { - regs[i] = DINA80 + 3; - } - } - result = inv_set_mpu_memory(KEY_CFG_4, 8, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - cntrl_params.functions = function; - result = inv_set_dead_zone(); - - return result; -} - -/** - * @brief inv_get_control_signal is used to get the current control signal with - * high precision. - * inv_get_control_signal is used to acquire the current data of a control signal. - * If INV_GRID is being used, inv_get_grid_number will probably be preferrable. - * - * @param controlSignal Indicates which control signal is being queried. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param reset Indicates whether the control signal should be reset to zero. - * Options are INV_RESET or INV_NO_RESET - * @param data A pointer to the current control signal data. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ -inv_error_t inv_get_control_signal(unsigned short controlSignal, - unsigned short reset, long *data) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - *data = cntrl_obj.controlInt[0]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[0] = 0; - } - break; - case INV_CONTROL_2: - *data = cntrl_obj.controlInt[1]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[1] = 0; - } - break; - case INV_CONTROL_3: - *data = cntrl_obj.controlInt[2]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[2] = 0; - } - break; - case INV_CONTROL_4: - *data = cntrl_obj.controlInt[3]; - if (reset == INV_RESET) { - cntrl_obj.controlInt[3] = 0; - } - break; - default: - break; - } - return INV_SUCCESS; -} - -/** - * @brief inv_get_grid_num is used to get the current grid location for a certain - * control signal. - * inv_get_grid_num is used to acquire the current grid location. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being queried. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param reset Indicates whether the control signal should be reset to zero. - * Options are INV_RESET or INV_NO_RESET - * @param data A pointer to the current grid number. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset, - long *data) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - *data = cntrl_obj.gridNum[0]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[0] = 0; - } - break; - case INV_CONTROL_2: - *data = cntrl_obj.gridNum[1]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[1] = 0; - } - break; - case INV_CONTROL_3: - *data = cntrl_obj.gridNum[2]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[2] = 0; - } - break; - case INV_CONTROL_4: - *data = cntrl_obj.gridNum[3]; - if (reset == INV_RESET) { - cntrl_obj.gridNum[3] = 0; - } - break; - default: - break; - } - - return INV_SUCCESS; -} - -/** - * @brief inv_set_grid_thresh is used to set the grid size for a control signal. - * inv_set_grid_thresh is used to adjust the size of the grid being controlled. - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 and - * - INV_CONTROL_4. - * @param threshold The threshold of the control signal at which the grid - * number will be incremented or decremented. - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - cntrl_params.gridThreshold[0] = threshold; - break; - case INV_CONTROL_2: - cntrl_params.gridThreshold[1] = threshold; - break; - case INV_CONTROL_3: - cntrl_params.gridThreshold[2] = threshold; - break; - case INV_CONTROL_4: - cntrl_params.gridThreshold[3] = threshold; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - - return INV_SUCCESS; -} - -/** - * @brief inv_set_grid_max is used to set the maximum grid number for a control signal. - * inv_set_grid_max is used to adjust the maximum allowed grid number, above - * which the grid number will not be incremented. - * The minimum grid number is always zero. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 and - * - INV_CONTROL_4. - * - * @param maximum The maximum grid number for a control signal. - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - switch (controlSignal) { - case INV_CONTROL_1: - cntrl_params.gridMaximum[0] = maximum; - break; - case INV_CONTROL_2: - cntrl_params.gridMaximum[1] = maximum; - break; - case INV_CONTROL_3: - cntrl_params.gridMaximum[2] = maximum; - break; - case INV_CONTROL_4: - cntrl_params.gridMaximum[3] = maximum; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - break; - } - - return INV_SUCCESS; -} - -/** - * @brief GridCallback function pointer type, to be passed as argument of - * inv_set_grid_callback. - * - * @param controlSignal Indicates which control signal crossed a grid threshold. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 and - * - INV_CONTROL_4. - * - * @param gridNumber An array of four numbers representing the grid number for each - * control signal. - * @param gridChange An array of four numbers representing the change in grid number - * for each control signal. -**/ -typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum, - long *gridChange); - -/** - * @brief inv_set_grid_callback is used to register a callback function that - * will trigger when the grid location changes. - * inv_set_grid_callback allows a user to define a callback function that will - * run when a control signal crosses a grid threshold. - - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). inv_dmp_start must <b>NOT</b> have - * been called. - * - * @param func A user defined callback function - * @return Zero if the command is successful; an ML error code otherwise. -**/ -inv_error_t inv_set_grid_callback(fpGridCb func) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - cntrl_params.gridCallback = func; - return INV_SUCCESS; -} - -/** - * @brief inv_set_control_data is used to assign physical parameters to control signals. - * inv_set_control_data allows flexibility in assigning physical parameters to - * control signals. For example, the user is allowed to use raw gyroscope data - * as an input to the control algorithm. - * Alternatively, angular velocity can be used, which combines gyroscopes and - * accelerometers to provide a more robust physical parameter. Finally, angular - * velocity in world coordinates can be used, providing a control signal in - * which pitch and yaw are provided relative to gravity. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being modified. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param parameterArray Indicates which parameter array is being assigned to a - * control signal. Must be one of: - * - INV_GYROS, - * - INV_ANGULAR_VELOCITY, or - * - * @param parameterAxis Indicates which axis of the parameter array will be used. - * Must be: - * - INV_ROLL, - * - INV_PITCH, or - * - INV_YAW. - */ - -inv_error_t inv_set_control_data(unsigned short controlSignal, - unsigned short parameterArray, - unsigned short parameterAxis) -{ - INVENSENSE_FUNC_START; - unsigned char regs[2] = { DINA80 + 10, DINA20 }; - inv_error_t result; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - if (parameterArray == INV_ANGULAR_VELOCITY) { - regs[0] = DINA80 + 5; - regs[1] = DINA00; - } - switch (controlSignal) { - case INV_CONTROL_1: - cntrl_params.parameterArray[0] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += 0x02; - cntrl_params.parameterAxis[0] = 0; - break; - case INV_ROLL: - regs[1] = DINA22; - cntrl_params.parameterAxis[0] = 1; - break; - case INV_YAW: - regs[1] = DINA42; - cntrl_params.parameterAxis[0] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case INV_CONTROL_2: - cntrl_params.parameterArray[1] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += DINA0E; - cntrl_params.parameterAxis[1] = 0; - break; - case INV_ROLL: - regs[1] += DINA2E; - cntrl_params.parameterAxis[1] = 1; - break; - case INV_YAW: - regs[1] += DINA4E; - cntrl_params.parameterAxis[1] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case INV_CONTROL_3: - cntrl_params.parameterArray[2] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += DINA0E; - cntrl_params.parameterAxis[2] = 0; - break; - case INV_ROLL: - regs[1] += DINA2E; - cntrl_params.parameterAxis[2] = 1; - break; - case INV_YAW: - regs[1] += DINA4E; - cntrl_params.parameterAxis[2] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case INV_CONTROL_4: - cntrl_params.parameterArray[3] = parameterArray; - switch (parameterAxis) { - case INV_PITCH: - regs[1] += DINA0E; - cntrl_params.parameterAxis[3] = 0; - break; - case INV_ROLL: - regs[1] += DINA2E; - cntrl_params.parameterAxis[3] = 1; - break; - case INV_YAW: - regs[1] += DINA4E; - cntrl_params.parameterAxis[3] = 2; - break; - default: - return INV_ERROR_INVALID_PARAMETER; - } - result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} - -/** - * @brief inv_get_control_data is used to get the current control data. - * - * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or - * inv_open_low_power_pedometer(). - * - * @param controlSignal Indicates which control signal is being queried. - * Must be one of: - * - INV_CONTROL_1, - * - INV_CONTROL_2, - * - INV_CONTROL_3 or - * - INV_CONTROL_4. - * - * @param gridNum A pointer to pass gridNum info back to the user. - * @param gridChange A pointer to pass gridChange info back to the user. - * - * @return Zero if the command is successful; an ML error code otherwise. - */ - -inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, - long *gridChange) -{ - INVENSENSE_FUNC_START; - int_fast8_t i = 0; - - if (inv_get_state() != INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - for (i = 0; i < 4; i++) { - controlSignal[i] = cntrl_obj.controlInt[i]; - gridNum[i] = cntrl_obj.gridNum[i]; - gridChange[i] = cntrl_obj.gridChange[i]; - } - return INV_SUCCESS; -} - -/** - * @internal - * @brief Update the ML Control engine. This function should be called - * every time new data from the MPU becomes available. - * Control engine outputs are written to the cntrl_obj data - * structure. - * @return INV_SUCCESS or an error code. -**/ -inv_error_t inv_update_control(struct inv_obj_t * inv_obj) -{ - INVENSENSE_FUNC_START; - unsigned char i; - long gridTmp; - long tmp; - - inv_get_cntrl_data(cntrl_obj.mlGridNumDMP); - - for (i = 0; i < 4; i++) { - if (cntrl_params.functions & INV_GRID) { - if (cntrl_params.functions & INV_HYSTERESIS) { - cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i]; - } - cntrl_obj.mlGridNumDMP[i] = - cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L; - cntrl_obj.controlInt[i] = - (cntrl_obj.mlGridNumDMP[i] % - (128 * cntrl_params.gridThreshold[i])) / 128; - gridTmp = - cntrl_obj.mlGridNumDMP[i] / (128 * - cntrl_params.gridThreshold[i]); - tmp = 1 + 16777216L / cntrl_params.gridThreshold[i]; - cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i]; - if (cntrl_obj.gridChange[i] > tmp / 2) { - cntrl_obj.gridChange[i] = - gridTmp - tmp - cntrl_obj.lastGridNum[i]; - } else if (cntrl_obj.gridChange[i] < -tmp / 2) { - cntrl_obj.gridChange[i] = - gridTmp + tmp - cntrl_obj.lastGridNum[i]; - } - if ((cntrl_params.functions & INV_HYSTERESIS) - && (cntrl_obj.gridChange[i] != 0)) { - if (cntrl_obj.gridChange[i] > 0) { - cntrl_obj.gridNumOffset[i] += - 128 * cntrl_params.gridThreshold[i]; - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - if (cntrl_obj.gridChange[i] < 0) { - cntrl_obj.gridNumOffset[i] -= - 128 * cntrl_params.gridThreshold[i]; - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - } - cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i]; - if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) { - cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i]; - if (cntrl_obj.controlInt[i] >= - cntrl_params.gridThreshold[i] / 2) { - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - } else if (cntrl_obj.gridNum[i] <= 0) { - cntrl_obj.gridNum[i] = 0; - if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) { - cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; - } - } - cntrl_obj.lastGridNum[i] = gridTmp; - if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) { - cntrl_params.gridCallback((INV_CONTROL_1 << i), - cntrl_obj.gridNum, - cntrl_obj.gridChange); - } - - } else { - cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i]; - } - - } - - return INV_SUCCESS; -} - -/** - * @brief Enables the INV_CONTROL engine. - * - * @note This function replaces MLEnable(INV_CONTROL) - * - * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must - * have been called. - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_enable_control(void) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() != INV_STATE_DMP_OPENED) - return INV_ERROR_SM_IMPROPER_STATE; - - memset(&cntrl_obj, 0, sizeof(cntrl_obj)); - - inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo - return INV_SUCCESS; -} - -/** - * @brief Disables the INV_CONTROL engine. - * - * @note This function replaces MLDisable(INV_CONTROL) - * - * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must - * have been called. - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_disable_control(void) -{ - INVENSENSE_FUNC_START; - - if (inv_get_state() < INV_STATE_DMP_STARTED) - return INV_ERROR_SM_IMPROPER_STATE; - - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/mlcontrol.h b/60xx/mlsdk/mllite/mlcontrol.h deleted file mode 100644 index a834fc6..0000000 --- a/60xx/mlsdk/mllite/mlcontrol.h +++ /dev/null @@ -1,217 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $RCSfile: mlcontrol.h,v $ - * - * $Date: 2011-06-10 20:13:08 -0700 (Fri, 10 Jun 2011) $ - * - * $Revision: 5629 $ - * - *******************************************************************************/ - -/*******************************************************************************/ -/** @defgroup INV_CONTROL - - The Control processes gyroscopes and accelerometers to provide control - signals that can be used in user interfaces to manipulate objects such as - documents, images, cursors, menus, etc. - - @{ - @file mlcontrol.h - @brief Header file for the Control Library. -*/ -/******************************************************************************/ -#ifndef MLCONTROL_H -#define MLCONTROL_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "ml.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlcontrol_legacy.h" -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /*******************************************************************************/ - /* Control Signals. */ - /*******************************************************************************/ - -#define INV_CONTROL_1 0x0001 -#define INV_CONTROL_2 0x0002 -#define INV_CONTROL_3 0x0004 -#define INV_CONTROL_4 0x0008 - - /*******************************************************************************/ - /* Control Functions. */ - /*******************************************************************************/ - -#define INV_GRID 0x0001 // Indicates that the user will be controlling a system that - // has discrete steps, such as icons, menu entries, pixels, etc. -#define INV_SMOOTH 0x0002 // Indicates that noise from unintentional motion should be filtered out. -#define INV_DEAD_ZONE 0x0004 // Indicates that a dead zone should be used, below which sensor data is set to zero. -#define INV_HYSTERESIS 0x0008 // Indicates that, when INV_GRID is selected, hysteresis should be used to prevent - // the control signal from switching rapidly across elements of the grid.</dd> - - /*******************************************************************************/ - /* Integral reset options. */ - /*******************************************************************************/ - -#define INV_NO_RESET 0x0000 -#define INV_RESET 0x0001 - - /*******************************************************************************/ - /* Data select options. */ - /*******************************************************************************/ - -#define INV_CTRL_SIGNAL 0x0000 -#define INV_CTRL_GRID_NUM 0x0001 - - /*******************************************************************************/ - /* Control Axis. */ - /*******************************************************************************/ -#define INV_CTRL_PITCH 0x0000 // (INV_PITCH >> 1) -#define INV_CTRL_ROLL 0x0001 // (INV_ROLL >> 1) -#define INV_CTRL_YAW 0x0002 // (INV_YAW >> 1) - - /*******************************************************************************/ - /* control_params structure default values. */ - /*******************************************************************************/ - -#define MLCTRL_SENSITIVITY_0_DEFAULT 128 -#define MLCTRL_SENSITIVITY_1_DEFAULT 128 -#define MLCTRL_SENSITIVITY_2_DEFAULT 128 -#define MLCTRL_SENSITIVITY_3_DEFAULT 128 -#define MLCTRL_FUNCTIONS_DEFAULT 0 -#define MLCTRL_CONTROL_SIGNALS_DEFAULT 0 -#define MLCTRL_PARAMETER_ARRAY_0_DEFAULT 0 -#define MLCTRL_PARAMETER_ARRAY_1_DEFAULT 0 -#define MLCTRL_PARAMETER_ARRAY_2_DEFAULT 0 -#define MLCTRL_PARAMETER_ARRAY_3_DEFAULT 0 -#define MLCTRL_PARAMETER_AXIS_0_DEFAULT 0 -#define MLCTRL_PARAMETER_AXIS_1_DEFAULT 0 -#define MLCTRL_PARAMETER_AXIS_2_DEFAULT 0 -#define MLCTRL_PARAMETER_AXIS_3_DEFAULT 0 -#define MLCTRL_GRID_THRESHOLD_0_DEFAULT 1 -#define MLCTRL_GRID_THRESHOLD_1_DEFAULT 1 -#define MLCTRL_GRID_THRESHOLD_2_DEFAULT 1 -#define MLCTRL_GRID_THRESHOLD_3_DEFAULT 1 -#define MLCTRL_GRID_MAXIMUM_0_DEFAULT 0 -#define MLCTRL_GRID_MAXIMUM_1_DEFAULT 0 -#define MLCTRL_GRID_MAXIMUM_2_DEFAULT 0 -#define MLCTRL_GRID_MAXIMUM_3_DEFAULT 0 -#define MLCTRL_GRID_CALLBACK_DEFAULT 0 - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /**************************************************************************/ - /* Control Parameters Structure. */ - /**************************************************************************/ - - struct control_params { - // Sensitivity of control signal 1, 2, 3, and 4. - unsigned short sensitivity[4]; - // Indicates what functions will be used. Can be a bitwise OR of INV_GRID, - // ML_SMOOT, INV_DEAD_ZONE, and INV_HYSTERISIS. - unsigned short functions; - // Indicates which parameter array is being assigned to a control signal. - // Must be one of INV_GYROS, INV_ANGULAR_VELOCITY, or - // INV_ANGULAR_VELOCITY_WORLD. - unsigned short parameterArray[4]; - // Indicates which axis of the parameter array will be used. Must be - // INV_ROLL, INV_PITCH, or INV_YAW. - unsigned short parameterAxis[4]; - // Threshold of the control signal at which the grid number will be - // incremented or decremented. - long gridThreshold[4]; - // Maximum grid number for the control signal. - long gridMaximum[4]; - // User defined callback that will trigger when the grid location changes. - void (*gridCallback) ( - // Indicates which control signal crossed a grid threshold. Must be - // one of INV_CONTROL_1, INV_CONTROL_2, INV_CONTROL_3 or INV_CONTROL_4. - unsigned short controlSignal, - // An array of four numbers representing the grid number for each - // control signal. - long *gridNum, - // An array of four numbers representing the change in grid number - // for each control signal. - long *gridChange); - }; - - struct control_obj { - - long gridNum[4]; // Current grid number for each control signal. - long controlInt[4]; // Current data for each control signal. - long lastGridNum[4]; // Previous grid number - unsigned char controlDir[4]; // Direction of control signal - long gridChange[4]; // Change in grid number - - long mlGridNumDMP[4]; - long gridNumOffset[4]; - long prevDMPGridNum[4]; - - }; - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - - /**************************************************************************/ - /* ML Control Functions. */ - /**************************************************************************/ - - unsigned short inv_get_control_params(struct control_params *params); - unsigned short inv_set_control_params(struct control_params *params); - - /*API for handling control signals */ - inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, - long sensitivity); - inv_error_t inv_set_control_func(unsigned short function); - inv_error_t inv_get_control_signal(unsigned short controlSignal, - unsigned short reset, long *data); - inv_error_t inv_get_grid_num(unsigned short controlSignal, - unsigned short reset, long *data); - inv_error_t inv_set_grid_thresh(unsigned short controlSignal, - long threshold); - inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum); - inv_error_t - inv_set_grid_callback(void (*func) - (unsigned short controlSignal, long *gridNum, - long *gridChange)); - inv_error_t inv_set_control_data(unsigned short controlSignal, - unsigned short parameterArray, - unsigned short parameterNum); - inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, - long *gridChange); - inv_error_t inv_update_control(struct inv_obj_t *inv_obj); - inv_error_t inv_enable_control(void); - inv_error_t inv_disable_control(void); - -#ifdef __cplusplus -} -#endif -#endif /* MLCONTROL_H */ diff --git a/60xx/mlsdk/mllite/mldl.c b/60xx/mlsdk/mllite/mldl.c deleted file mode 100644 index 7054532..0000000 --- a/60xx/mlsdk/mllite/mldl.c +++ /dev/null @@ -1,1092 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * - * $Id: mldl.c 5653 2011-06-16 21:06:55Z nroyer $ - * - *****************************************************************************/ - -/** - * @defgroup MLDL - * @brief Motion Library - Driver Layer. - * The Motion Library Driver Layer provides the intrface to the - * system drivers that are used by the Motion Library. - * - * @{ - * @file mldl.c - * @brief The Motion Library Driver Layer. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> - -#include "mpu.h" -#if defined CONFIG_MPU_SENSORS_MPU6050A2 -# include "mpu6050a2.h" -#elif defined CONFIG_MPU_SENSORS_MPU6050B1 -# include "mpu6050b1.h" -#elif defined CONFIG_MPU_SENSORS_MPU3050 -# include "mpu3050.h" -#else -#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx -#endif -#include "mldl.h" -#include "mldl_cfg.h" -#include "compass.h" -#include "mlsl.h" -#include "mlos.h" -#include "mlinclude.h" -#include "ml.h" -#include "dmpKey.h" -#include "mlFIFOHW.h" -#include "compass.h" -#include "pressure.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mldl" - -#define _mldlDebug(x) //{x} - -/* --------------------- */ -/* - Variables. - */ -/* --------------------- */ - -#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */ - -/*---- structure containing control variables used by MLDL ----*/ -static struct mldl_cfg mldlCfg; -struct ext_slave_descr gAccel; -struct ext_slave_descr gCompass; -struct ext_slave_descr gPressure; -struct mpu_platform_data gPdata; -static void *sMLSLHandle; -int_fast8_t intTrigger[NUM_OF_INTSOURCES]; - -/******************************************************************************* - * Functions for accessing the DMP memory via keys - ******************************************************************************/ - -unsigned short (*sGetAddress) (unsigned short key) = NULL; -static const unsigned char *localDmpMemory = NULL; -static unsigned short localDmpMemorySize = 0; - -/** - * @internal - * @brief Sets the function to use to convert keys to addresses. This - * will changed for each DMP code loaded. - * @param func - * Function used to convert keys to addresses. - * @endif - */ -void inv_set_get_address(unsigned short (*func) (unsigned short key)) -{ - INVENSENSE_FUNC_START; - _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func); - ) - sGetAddress = func; -} - -/** - * @internal - * @brief Check if the feature is supported in the currently loaded - * DMP code basing on the fact that the key is assigned a - * value or not. - * @param key the DMP key - * @return whether the feature associated with the key is supported - * or not. - */ -uint_fast8_t inv_dmpkey_supported(unsigned short key) -{ - unsigned short memAddr; - - if (sGetAddress == NULL) { - MPL_LOGE("%s : sGetAddress is NULL\n", __func__); - return FALSE; - } - - memAddr = sGetAddress(key); - if (memAddr >= 0xffff) { - MPL_LOGV("inv_set_mpu_memory unsupported key\n"); - return FALSE; - } - - return TRUE; -} - -/** - * @internal - * @brief used to get the specified number of bytes from the original - * MPU memory location specified by the key. - * Reads the specified number of bytes from the MPU location - * that was used to program the MPU specified by the key. Each - * set of code specifies a function that changes keys into - * addresses. This function is set with setGetAddress(). - * - * @param key The key to use when looking up the address. - * @param length Number of bytes to read. - * @param buffer Result for data. - * - * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key - * not corresponding to a memory address will result in INV_ERROR. - * @endif - */ -inv_error_t inv_get_mpu_memory_original(unsigned short key, - unsigned short length, - unsigned char *buffer) -{ - unsigned short offset; - - if (sGetAddress == NULL) { - return INV_ERROR_NOT_OPENED; - } - - offset = sGetAddress(key); - if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) { - return INV_ERROR_INVALID_PARAMETER; - } - - memcpy(buffer, &localDmpMemory[offset], length); - - return INV_SUCCESS; -} - -unsigned short inv_dl_get_address(unsigned short key) -{ - unsigned short offset; - if (sGetAddress == NULL) { - return INV_ERROR_NOT_OPENED; - } - - offset = sGetAddress(key); - return offset; -} - -/* ---------------------- */ -/* - Static Functions. - */ -/* ---------------------- */ - -/** - * @brief Open the driver layer and resets the internal - * gyroscope, accelerometer, and compass data - * structures. - * @param mlslHandle - * the serial handle. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_dl_open(void *mlslHandle) -{ - inv_error_t result; - memset(&mldlCfg, 0, sizeof(mldlCfg)); - memset(intTrigger, INT_CLEAR, sizeof(intTrigger)); - - sMLSLHandle = mlslHandle; - - mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */ - mldlCfg.accel = &gAccel; - mldlCfg.compass = &gCompass; - mldlCfg.pressure = &gPressure; - mldlCfg.pdata = &gPdata; - - result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle, - sMLSLHandle, sMLSLHandle, sMLSLHandle); - return result; -} - -/** - * @brief Closes/Cleans up the ML Driver Layer. - * Put the device in sleep mode. - * @return INV_SUCCESS or non-zero error code. - */ -inv_error_t inv_dl_close(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - result = (inv_error_t) inv_mpu_suspend(&mldlCfg, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - INV_ALL_SENSORS); - - result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle, - sMLSLHandle, sMLSLHandle, sMLSLHandle); - /* Clear all previous settings */ - memset(&mldlCfg, 0, sizeof(mldlCfg)); - sMLSLHandle = NULL; - sGetAddress = NULL; - return result; -} - -/** - * @brief Sets the requested_sensors - * - * Accessor to set the requested_sensors field of the mldl_cfg structure. - * Typically set at initialization. - * - * @param sensors - * Bitfield of the sensors that are going to be used. Combination of the - * following: - * - INV_X_GYRO - * - INV_Y_GYRO - * - INV_Z_GYRO - * - INV_DMP_PROCESSOR - * - INV_X_ACCEL - * - INV_Y_ACCEL - * - INV_Z_ACCEL - * - INV_X_COMPASS - * - INV_Y_COMPASS - * - INV_Z_COMPASS - * - INV_X_PRESSURE - * - INV_Y_PRESSURE - * - INV_Z_PRESSURE - * - INV_THREE_AXIS_GYRO - * - INV_THREE_AXIS_ACCEL - * - INV_THREE_AXIS_COMPASS - * - INV_THREE_AXIS_PRESSURE - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_init_requested_sensors(unsigned long sensors) -{ - mldlCfg.requested_sensors = sensors; - - return INV_SUCCESS; -} - -/** - * @brief Starts the DMP running - * - * Resumes the sensor if any of the sensor axis or components are requested - * - * @param sensors - * Bitfield of the sensors to turn on. Combination of the following: - * - INV_X_GYRO - * - INV_Y_GYRO - * - INV_Z_GYRO - * - INV_DMP_PROCESSOR - * - INV_X_ACCEL - * - INV_Y_ACCEL - * - INV_Z_ACCEL - * - INV_X_COMPASS - * - INV_Y_COMPASS - * - INV_Z_COMPASS - * - INV_X_PRESSURE - * - INV_Y_PRESSURE - * - INV_Z_PRESSURE - * - INV_THREE_AXIS_GYRO - * - INV_THREE_AXIS_ACCEL - * - INV_THREE_AXIS_COMPASS - * - INV_THREE_AXIS_PRESSURE - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_dl_start(unsigned long sensors) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - mldlCfg.requested_sensors = sensors; - result = inv_mpu_resume(&mldlCfg, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sensors); - return result; -} - -/** - * @brief Stops the DMP running and puts it in low power as requested - * - * Suspends each sensor according to the bitfield, if all axis and components - * of the sensor is off. - * - * @param sensors Bitfiled of the sensors to leave on. Combination of the - * following: - * - INV_X_GYRO - * - INV_Y_GYRO - * - INV_Z_GYRO - * - INV_X_ACCEL - * - INV_Y_ACCEL - * - INV_Z_ACCEL - * - INV_X_COMPASS - * - INV_Y_COMPASS - * - INV_Z_COMPASS - * - INV_X_PRESSURE - * - INV_Y_PRESSURE - * - INV_Z_PRESSURE - * - INV_THREE_AXIS_GYRO - * - INV_THREE_AXIS_ACCEL - * - INV_THREE_AXIS_COMPASS - * - INV_THREE_AXIS_PRESSURE - * - * - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t inv_dl_stop(unsigned long sensors) -{ - INVENSENSE_FUNC_START; - inv_error_t result = INV_SUCCESS; - - result = inv_mpu_suspend(&mldlCfg, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sMLSLHandle, - sensors); - return result; -} - -/** - * @brief Get a pointer to the internal data structure - * storing the configuration for the MPU, the accelerometer - * and the compass in use. - * @return a pointer to the data structure of type 'struct mldl_cfg'. - */ -struct mldl_cfg *inv_get_dl_config(void) -{ - return &mldlCfg; -} - -/** - * @brief Query the MPU slave address. - * @return The 7-bit mpu slave address. - */ -unsigned char inv_get_mpu_slave_addr(void) -{ - INVENSENSE_FUNC_START; - return mldlCfg.addr; -} - -/** - * @internal - * @brief MLDLCfgDMP configures the Digital Motion Processor internal to - * the MPU. The DMP can be enabled or disabled and the start address - * can be set. - * - * @param enableRun Enables the DMP processing if set to TRUE. - * @param enableFIFO Enables DMP output to the FIFO if set to TRUE. - * @param startAddress start address - * - * @return Zero if the command is successful, an error code otherwise. -*/ -inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, - unsigned char enableFIFO) -{ - INVENSENSE_FUNC_START; - - mldlCfg.dmp_enable = enableRun; - mldlCfg.fifo_enable = enableFIFO; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin. - * The basic interrupt signal characteristics can be set - * (i.e. active high/low, open drain/push pull, etc.) and the - * triggers can be set. - * Currently only INTPIN_MPU is supported. - * - * @param triggers - * bitmask of triggers to enable for interrupt. - * The available triggers are: - * - BIT_MPU_RDY_EN - * - BIT_DMP_INT_EN - * - BIT_RAW_RDY_EN - * - * @return Zero if the command is successful, an error code otherwise. -*/ -inv_error_t inv_get_dl_cfg_int(unsigned char triggers) -{ - inv_error_t result = INV_SUCCESS; - -#if defined CONFIG_MPU_SENSORS_MPU3050 - /* Mantis has 8 bits of interrupt config bits */ - if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) { - return INV_ERROR_INVALID_PARAMETER; - } -#endif - - mldlCfg.int_config = triggers; - if (!mldlCfg.gyro_is_suspended) { - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_INT_CFG, - (mldlCfg.int_config | mldlCfg.pdata-> - int_config)); - } else { - mldlCfg.gyro_needs_reset = TRUE; - } - - return result; -} - -/** - * @brief configures the output sampling rate on the MPU. - * Three parameters control the sampling: - * - * 1) Low pass filter bandwidth, and - * 2) output sampling divider. - * - * The output sampling rate is determined by the divider and the low - * pass filter setting. If the low pass filter is set to - * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the - * divider is 8kHz; for all other settings it is 1kHz. - * The 8-bit divider will divide this frequency to get the resulting - * sample frequency. - * For example, if the filter setting is not 256Hz and the divider is - * set to 7, then the sample rate is as follows: - * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms). - * - * The low pass filter selection codes control both the cutoff frequency of - * the internal low pass filter and internal analog sampling rate. The - * latter, in turn, affects the final output sampling rate according to the - * sample rate divider settig. - * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate, - * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate, - * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate, - * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate, - * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate, - * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate, - * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate, - * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate. - * - * @param lpf low pass filter, 0 to 7. - * @param divider Output sampling rate divider, 0 to 255. - * - * @return ML_SUCESS if successful; a non-zero error code otherwise. -**/ -inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider) -{ - /*---- do range checking ----*/ - if (lpf >= NUM_MPU_FILTER) { - return INV_ERROR_INVALID_PARAMETER; - } - - mldlCfg.lpf = lpf; - mldlCfg.divider = divider; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief set the full scale range for the gyros. - * The full scale selection codes correspond to: - * 0 -> 250 dps, - * 1 -> 500 dps, - * 2 -> 1000 dps, - * 3 -> 2000 dps. - * Full scale range affect the MPU's measurement - * sensitivity. - * - * @param fullScale - * the gyro full scale range in dps. - * - * @return INV_SUCCESS or non-zero error code. -**/ -inv_error_t inv_set_full_scale(float fullScale) -{ - if (fullScale == 250.f) - mldlCfg.full_scale = MPU_FS_250DPS; - else if (fullScale == 500.f) - mldlCfg.full_scale = MPU_FS_500DPS; - else if (fullScale == 1000.f) - mldlCfg.full_scale = MPU_FS_1000DPS; - else if (fullScale == 2000.f) - mldlCfg.full_scale = MPU_FS_2000DPS; - else { // not a valid setting - MPL_LOGE("Invalid full scale range specification for gyros : %f\n", - fullScale); - MPL_LOGE - ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n"); - return INV_ERROR_INVALID_PARAMETER; - } - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief This function sets the external sync for the MPU sampling. - * It can be synchronized on the LSB of any of the gyros, any of the - * external accels, or on the temp readings. - * - * @param extSync External sync selection, 0 to 7. - * @return Zero if the command is successful; an error code otherwise. -**/ -inv_error_t inv_set_external_sync(unsigned char extSync) -{ - INVENSENSE_FUNC_START; - - /*---- do range checking ----*/ - if (extSync >= NUM_MPU_EXT_SYNC) { - return INV_ERROR_INVALID_PARAMETER; - } - mldlCfg.ext_sync = extSync; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -inv_error_t inv_set_ignore_system_suspend(unsigned char ignore) -{ - INVENSENSE_FUNC_START; - - mldlCfg.ignore_system_suspend = ignore; - - return INV_SUCCESS; -} - -/** - * @brief inv_clock_source function sets the clock source for the MPU gyro - * processing. - * The source can be any of the following: - * - Internal 8MHz oscillator, - * - PLL with X gyro as reference, - * - PLL with Y gyro as reference, - * - PLL with Z gyro as reference, - * - PLL with external 32.768Mhz reference, or - * - PLL with external 19.2MHz reference - * - * For best accuracy and timing, it is highly recommended to use one - * of the gyros as the clock source; however this gyro must be - * enabled to use its clock (see 'MLDLPowerMgmtMPU()'). - * - * @param clkSource Clock source selection. - * Can be one of: - * - CLK_INTERNAL, - * - CLK_PLLGYROX, - * - CLK_PLLGYROY, - * - CLK_PLLGYROZ, - * - CLK_PLLEXT32K, or - * - CLK_PLLEXT19M. - * - * @return Zero if the command is successful; an error code otherwise. -**/ -inv_error_t inv_clock_source(unsigned char clkSource) -{ - INVENSENSE_FUNC_START; - - /*---- do range checking ----*/ - if (clkSource >= NUM_CLK_SEL) { - return INV_ERROR_INVALID_PARAMETER; - } - - mldlCfg.clk_src = clkSource; - mldlCfg.gyro_needs_reset = TRUE; - - return INV_SUCCESS; -} - -/** - * @brief Set the Temperature Compensation offset. - * @param tc - * a pointer to the temperature compensations offset - * for the 3 gyro axes. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_set_offsetTC(const unsigned char *tc) -{ - int ii; - inv_error_t result; - - for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) { - mldlCfg.offset_tc[ii] = tc[ii]; - } - - if (!mldlCfg.gyro_is_suspended) { -#ifdef CONFIG_MPU_SENSORS_MPU3050 - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_XG_OFFS_TC, tc[0]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_YG_OFFS_TC, tc[1]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_ZG_OFFS_TC, tc[2]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#else - unsigned char reg; - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_XG_OFFS_TC, - ((mldlCfg.offset_tc[0] << 1) - & BITS_XG_OFFS_TC)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC); -#ifdef CONFIG_MPU_SENSORS_MPU6050B1 - if (mldlCfg.pdata->level_shifter) - reg |= BIT_I2C_MST_VDDIO; -#endif - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_YG_OFFS_TC, reg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, - MPUREG_ZG_OFFS_TC, - ((mldlCfg.offset_tc[2] << 1) - & BITS_ZG_OFFS_TC)); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#endif - } else { - mldlCfg.gyro_needs_reset = TRUE; - } - return INV_SUCCESS; -} - -/** - * @brief Set the gyro offset. - * @param offset - * a pointer to the gyro offset for the 3 gyro axes. This is scaled - * as it would be written to the hardware registers. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_set_offset(const short *offset) -{ - inv_error_t result; - unsigned char regs[7]; - int ii; - long sf; - - sf = (2000L * 131) / mldlCfg.gyro_sens_trim; - for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) { - // Record the bias in the units the register uses - mldlCfg.offset[ii] = offset[ii]; - // Convert the bias to 1 dps = 1<<16 - inv_obj.gyro_bias[ii] = -offset[ii] * sf; - regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff; - regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff); - } - - if (!mldlCfg.gyro_is_suspended) { - regs[0] = MPUREG_X_OFFS_USRH; - result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else { - mldlCfg.gyro_needs_reset = TRUE; - } - return INV_SUCCESS; -} - -/** - * @internal - * @brief used to get the specified number of bytes in the specified MPU - * memory bank. - * The memory bank is one of the following: - * - MPUMEM_RAM_BANK_0, - * - MPUMEM_RAM_BANK_1, - * - MPUMEM_RAM_BANK_2, or - * - MPUMEM_RAM_BANK_3. - * - * @param bank Memory bank to write. - * @param memAddr Starting address for write. - * @param length Number of bytes to write. - * @param buffer Result for data. - * - * @return zero if the command is successful, an error code otherwise. - * @endif - */ -inv_error_t -inv_get_mpu_memory_one_bank(unsigned char bank, - unsigned char memAddr, - unsigned short length, unsigned char *buffer) -{ - inv_error_t result; - - if ((bank >= MPU_MEM_NUM_RAM_BANKS) || - //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255 - ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (mldlCfg.gyro_is_suspended) { - memcpy(buffer, &mldlCfg.ram[bank][memAddr], length); - result = INV_SUCCESS; - } else { - result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr, - ((bank << 8) | memAddr), length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - return result; -} - -/** - * @internal - * @brief used to set the specified number of bytes in the specified MPU - * memory bank. - * The memory bank is one of the following: - * - MPUMEM_RAM_BANK_0, - * - MPUMEM_RAM_BANK_1, - * - MPUMEM_RAM_BANK_2, or - * - MPUMEM_RAM_BANK_3. - * - * @param bank Memory bank to write. - * @param memAddr Starting address for write. - * @param length Number of bytes to write. - * @param buffer Result for data. - * - * @return zero if the command is successful, an error code otherwise. - * @endif - */ -inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, - unsigned short memAddr, - unsigned short length, - const unsigned char *buffer) -{ - inv_error_t result = INV_SUCCESS; - int different; - - if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) || - ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { - return INV_ERROR_INVALID_PARAMETER; - } - - different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length); - memcpy(&mldlCfg.ram[bank][memAddr], buffer, length); - if (!mldlCfg.gyro_is_suspended) { - result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr, - ((bank << 8) | memAddr), length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else if (different) { - mldlCfg.gyro_needs_reset = TRUE; - } - - return result; -} - -/** - * @internal - * @brief used to get the specified number of bytes from the MPU location - * specified by the key. - * Reads the specified number of bytes from the MPU location - * specified by the key. Each set of code specifies a function - * that changes keys into addresses. This function is set with - * setGetAddress(). - * - * @param key The key to use when looking up the address. - * @param length Number of bytes to read. - * @param buffer Result for data. - * - * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key - * not corresponding to a memory address will result in INV_ERROR. - * @endif - */ -inv_error_t inv_get_mpu_memory(unsigned short key, - unsigned short length, unsigned char *buffer) -{ - unsigned char bank; - inv_error_t result; - unsigned short memAddr; - - if (sGetAddress == NULL) { - return INV_ERROR_NOT_OPENED; - } - - memAddr = sGetAddress(key); - if (memAddr >= 0xffff) - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - bank = memAddr >> 8; // Get Bank - memAddr &= 0xff; - - while (memAddr + length > MPU_MEM_BANK_SIZE) { - // We cross a bank in the middle - unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; - result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, - sub_length, buffer); - if (INV_SUCCESS != result) - return result; - bank++; - length -= sub_length; - buffer += sub_length; - memAddr = 0; - } - result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, - length, buffer); - - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/** - * @internal - * @brief used to set the specified number of bytes from the MPU location - * specified by the key. - * Set the specified number of bytes from the MPU location - * specified by the key. Each set of DMP code specifies a function - * that changes keys into addresses. This function is set with - * setGetAddress(). - * - * @param key The key to use when looking up the address. - * @param length Number of bytes to write. - * @param buffer Result for data. - * - * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key - * not corresponding to a memory address will result in INV_ERROR. - * @endif - */ -inv_error_t inv_set_mpu_memory(unsigned short key, - unsigned short length, - const unsigned char *buffer) -{ - inv_error_t result = INV_SUCCESS; - unsigned short memAddr; - unsigned char bank; - - if (sGetAddress == NULL) { - MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n"); - return INV_ERROR_INVALID_MODULE; - } - memAddr = sGetAddress(key); - - if (memAddr >= 0xffff) { - MPL_LOGE("inv_set_mpu_memory unsupported key\n"); - return INV_ERROR_INVALID_MODULE; // This key not supported - } - - bank = (unsigned char)(memAddr >> 8); - memAddr &= 0xff; - - while (memAddr + length > MPU_MEM_BANK_SIZE) { - // We cross a bank in the middle - unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; - - result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - bank++; - length -= sub_length; - buffer += sub_length; - memAddr = 0; - } - result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * @brief Load the DMP with the given code and configuration. - * @param buffer - * the DMP data. - * @param length - * the length in bytes of the DMP data. - * @param config - * the DMP configuration. - * @return INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_load_dmp(const unsigned char *buffer, - unsigned short length, unsigned short config) -{ - INVENSENSE_FUNC_START; - - inv_error_t result = INV_SUCCESS; - unsigned short toWrite; - unsigned short memAddr = 0; - localDmpMemory = buffer; - localDmpMemorySize = length; - - mldlCfg.dmp_cfg1 = (config >> 8); - mldlCfg.dmp_cfg2 = (config & 0xff); - - while (length > 0) { - toWrite = length; - if (toWrite > MAX_LOAD_WRITE_SIZE) - toWrite = MAX_LOAD_WRITE_SIZE; - - result = - inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite, - buffer); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - buffer += toWrite; - memAddr += toWrite; - length -= toWrite; - } - - return result; -} - -/** - * @brief Get the silicon revision ID. - * @return The silicon revision ID - * (0 will be read if inv_mpu_open returned an error) - */ -unsigned char inv_get_silicon_rev(void) -{ - return mldlCfg.silicon_revision; -} - -/** - * @brief Get the product revision ID. - * @return The product revision ID - * (0 will be read if inv_mpu_open returned an error) - */ -unsigned char inv_get_product_rev(void) -{ - return mldlCfg.product_revision; -} - -/******************************************************************************* - ******************************************************************************* - ******************************************************************************* - * @todo these belong with an interface to the kernel driver layer - ******************************************************************************* - ******************************************************************************* - ******************************************************************************/ - -/** - * @brief inv_get_interrupt_status returns the interrupt status from the specified - * interrupt pin. - * @param intPin - * Currently only the value INTPIN_MPU is supported. - * @param status - * The available statuses are: - * - BIT_MPU_RDY_EN - * - BIT_DMP_INT_EN - * - BIT_RAW_RDY_EN - * - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_interrupt_status(unsigned char intPin, - unsigned char *status) -{ - INVENSENSE_FUNC_START; - - inv_error_t result; - - switch (intPin) { - - case INTPIN_MPU: - /*---- return the MPU interrupt status ----*/ - result = inv_serial_read(sMLSLHandle, mldlCfg.addr, - MPUREG_INT_STATUS, 1, status); - break; - - default: - result = INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} - -/** - * @brief query the current status of an interrupt source. - * @param srcIndex - * index of the interrupt source. - * Currently the only source supported is INTPIN_MPU. - * - * @return 1 if the interrupt has been triggered. - */ -unsigned char inv_get_interrupt_trigger(unsigned char srcIndex) -{ - INVENSENSE_FUNC_START; - return intTrigger[srcIndex]; -} - -/** - * @brief clear the 'triggered' status for an interrupt source. - * @param srcIndex - * index of the interrupt source. - * Currently only INTPIN_MPU is supported. - */ -void inv_clear_interrupt_trigger(unsigned char srcIndex) -{ - INVENSENSE_FUNC_START; - intTrigger[srcIndex] = 0; -} - -/** - * @brief inv_interrupt_handler function should be called when an interrupt is - * received. The source parameter identifies which interrupt source - * caused the interrupt. Note that this routine should not be called - * directly from the interrupt service routine. - * - * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1, - * INTSRC_AUX2, or INT_SRC_TIMER. - * - * @return Zero if the command is successful; an error code otherwise. - */ -inv_error_t inv_interrupt_handler(unsigned char intSource) -{ - INVENSENSE_FUNC_START; - /*---- range check ----*/ - if (intSource >= NUM_OF_INTSOURCES) { - return INV_ERROR; - } - - /*---- save source of interrupt ----*/ - intTrigger[intSource] = INT_TRIGGERED; - -#ifdef ML_USE_DMP_SIM - if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { - MLSimHWDataInput(); - } -#endif - - return INV_SUCCESS; -} - -/***************************/ - /**@}*//* end of defgroup */ -/***************************/ diff --git a/60xx/mlsdk/mllite/mldl.h b/60xx/mlsdk/mllite/mldl.h deleted file mode 100644 index 961d860..0000000 --- a/60xx/mlsdk/mllite/mldl.h +++ /dev/null @@ -1,176 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mldl.h 5639 2011-06-14 01:23:05Z nroyer $ - * - *******************************************************************************/ - -#ifndef MLDL_H -#define MLDL_H - -#include "mltypes.h" -#include "mlsl.h" -#include <linux/mpu.h> -#include "mldl_cfg.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mldl_legacy.h" -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -typedef enum _DEVICE_CONFIG { - DEVICE_MPU_ACCEL = 0, - DEVICE_MPU, - NUM_OF_DEVICES -} DEVICE_CONFIG; - -#define SERIAL_I2C 0 -#define SERIAL_SPI 1 - -#define DATAMODE_MANUAL 0 // Manual data mode -#define DATAMODE_AUTO 1 // Auto data mode - -#define DATASRC_IMMEDIATE 0 // Return data immediately -#define DATASRC_WHENREADY 1 // Only return data when new data is available -#define DATASRC_FIFO 2 // Use FIFO for data - -#define SENSOR_DATA_GYROX 0x0001 -#define SENSOR_DATA_GYROY 0x0002 -#define SENSOR_DATA_GYROZ 0x0004 -#define SENSOR_DATA_ACCELX 0x0008 -#define SENSOR_DATA_ACCELY 0x0010 -#define SENSOR_DATA_ACCELZ 0x0020 -#define SENSOR_DATA_AUX1 0x0040 -#define SENSOR_DATA_AUX2 0x0080 -#define SENSOR_DATA_AUX3 0x0100 -#define SENSOR_DATA_TEMP 0x0200 - -#define INTPIN_MPU 0 - -#define INTLOGIC_HIGH 0 -#define INTLOGIC_LOW 1 - -#define INTTYPE_PUSHPULL 0 -#define INTTYPE_OPENDRAIN 1 - -#define INTLATCH_DISABLE 0 -#define INTLATCH_ENABLE 1 - -#define MPUINT_MPU_READY 0x04 -#define MPUINT_DMP_DONE 0x02 -#define MPUINT_DATA_READY 0x01 - -#define INTLATCHCLEAR_READSTATUS 0 -#define INTLATCHCLEAR_ANYREAD 1 - -#define DMP_DONTRUN 0 -#define DMP_RUN 1 - - /*---- defines for external interrupt status (via external call into library) ----*/ -#define INT_CLEAR 0 -#define INT_TRIGGERED 1 - -typedef enum { - INTSRC_MPU = 0, - INTSRC_AUX1, - INTSRC_AUX2, - INTSRC_AUX3, - INTSRC_TIMER, - INTSRC_UNKNOWN, - INTSRC_MPU_FIFO, - INTSRC_MPU_MOTION, - NUM_OF_INTSOURCES, -} INT_SOURCE; - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------- */ - /* - Variables. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_dl_open(void *mlslHandle); - inv_error_t inv_dl_close(void); - - inv_error_t inv_dl_start(unsigned long sensors); - inv_error_t inv_dl_stop(unsigned long sensors); - - struct mldl_cfg *inv_get_dl_config(void); - - inv_error_t inv_init_requested_sensors(unsigned long sensors); - unsigned char inv_get_mpu_slave_addr(void); - inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, - unsigned char enableFIFO); - inv_error_t inv_get_dl_cfg_int(unsigned char triggers); - inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider); - inv_error_t inv_set_full_scale(float fullScale); - inv_error_t inv_set_external_sync(unsigned char extSync); - inv_error_t inv_set_ignore_system_suspend(unsigned char ignore); - inv_error_t inv_clock_source(unsigned char clkSource); - inv_error_t inv_get_mpu_memory(unsigned short key, - unsigned short length, - unsigned char *buffer); - inv_error_t inv_set_mpu_memory(unsigned short key, - unsigned short length, - const unsigned char *buffer); - inv_error_t inv_load_dmp(const unsigned char *buffer, - unsigned short length, - unsigned short startAddress); - - unsigned char inv_get_slicon_rev(void); - inv_error_t inv_set_offsetTC(const unsigned char *tc); - inv_error_t inv_set_offset(const short *offset); - - /* Functions for setting and retrieving the DMP memory */ - inv_error_t inv_get_mpu_memory_original(unsigned short key, - unsigned short length, - unsigned char *buffer); - void inv_set_get_address(unsigned short (*func) (unsigned short key)); - unsigned short inv_dl_get_address(unsigned short key); - uint_fast8_t inv_dmpkey_supported(unsigned short key); - - inv_error_t inv_get_interrupt_status(unsigned char intPin, - unsigned char *value); - unsigned char inv_get_interrupt_trigger(unsigned char index); - void inv_clear_interrupt_trigger(unsigned char index); - inv_error_t inv_interrupt_handler(unsigned char intSource); - - /** Only exposed for testing purposes */ - inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, - unsigned short memAddr, - unsigned short length, - const unsigned char *buffer); - inv_error_t inv_get_mpu_memory_one_bank(unsigned char bank, - unsigned char memAddr, - unsigned short length, - unsigned char *buffer); -#ifdef __cplusplus -} -#endif -#endif // MLDL_H diff --git a/60xx/mlsdk/mllite/mldl_cfg.h b/60xx/mlsdk/mllite/mldl_cfg.h deleted file mode 100644 index b4914e2..0000000 --- a/60xx/mlsdk/mllite/mldl_cfg.h +++ /dev/null @@ -1,336 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/** - * @addtogroup MLDL - * - * @{ - * @file mldl_cfg.h - * @brief The Motion Library Driver Layer Configuration header file. - */ - -#ifndef __MLDL_CFG_H__ -#define __MLDL_CFG_H__ - -#include "mltypes.h" -#include "mlsl.h" -#include <linux/mpu.h> -#if defined CONFIG_MPU_SENSORS_MPU6050A2 -# include "mpu6050a2.h" -#elif defined CONFIG_MPU_SENSORS_MPU6050B1 -# include "mpu6050b1.h" -#elif defined CONFIG_MPU_SENSORS_MPU3050 -# include "mpu3050.h" -#else -#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx -#endif - -#include "log.h" - -/************************************************************************* - * Sensors - *************************************************************************/ - -#define INV_X_GYRO (0x0001) -#define INV_Y_GYRO (0x0002) -#define INV_Z_GYRO (0x0004) -#define INV_DMP_PROCESSOR (0x0008) - -#define INV_X_ACCEL (0x0010) -#define INV_Y_ACCEL (0x0020) -#define INV_Z_ACCEL (0x0040) - -#define INV_X_COMPASS (0x0080) -#define INV_Y_COMPASS (0x0100) -#define INV_Z_COMPASS (0x0200) - -#define INV_X_PRESSURE (0x0300) -#define INV_Y_PRESSURE (0x0800) -#define INV_Z_PRESSURE (0x1000) - -#define INV_TEMPERATURE (0x2000) -#define INV_TIME (0x4000) - -#define INV_THREE_AXIS_GYRO (0x000F) -#define INV_THREE_AXIS_ACCEL (0x0070) -#define INV_THREE_AXIS_COMPASS (0x0380) -#define INV_THREE_AXIS_PRESSURE (0x1C00) - -#define INV_FIVE_AXIS (0x007B) -#define INV_SIX_AXIS_GYRO_ACCEL (0x007F) -#define INV_SIX_AXIS_ACCEL_COMPASS (0x03F0) -#define INV_NINE_AXIS (0x03FF) -#define INV_ALL_SENSORS (0x7FFF) - -#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev) - -/* -------------------------------------------------------------------------- */ - -/* Platform data for the MPU */ -struct mldl_cfg { - /* MPU related configuration */ - unsigned long requested_sensors; - unsigned char ignore_system_suspend; - unsigned char addr; - unsigned char int_config; - unsigned char ext_sync; - unsigned char full_scale; - unsigned char lpf; - unsigned char clk_src; - unsigned char divider; - unsigned char dmp_enable; - unsigned char fifo_enable; - unsigned char dmp_cfg1; - unsigned char dmp_cfg2; - unsigned char offset_tc[GYRO_NUM_AXES]; - unsigned short offset[GYRO_NUM_AXES]; - unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE]; - - /* MPU Related stored status and info */ - unsigned char product_revision; - unsigned char silicon_revision; - unsigned char product_id; - unsigned short gyro_sens_trim; -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - unsigned short accel_sens_trim; -#endif - - /* Driver/Kernel related state information */ - int gyro_is_bypassed; - int i2c_slaves_enabled; - int dmp_is_running; - int gyro_is_suspended; - int accel_is_suspended; - int compass_is_suspended; - int pressure_is_suspended; - int gyro_needs_reset; - - /* Slave related information */ - struct ext_slave_descr *accel; - struct ext_slave_descr *compass; - struct ext_slave_descr *pressure; - - /* Platform Data */ - struct mpu_platform_data *pdata; -}; - -/* -------------------------------------------------------------------------- */ - -int inv_mpu_open(struct mldl_cfg *mldl_cfg, - void *mlsl_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle); -int inv_mpu_close(struct mldl_cfg *mldl_cfg, - void *mlsl_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle); -int inv_mpu_resume(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle, - unsigned long sensors); -int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle, - unsigned long sensors); - -/* Slave Read functions */ -int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *slave_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - unsigned char *data); -static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *accel_handle, unsigned char *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_slave_read(mldl_cfg, gyro_handle, accel_handle, - mldl_cfg->accel, &mldl_cfg->pdata->accel, - data); -} - -static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *compass_handle, - unsigned char *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_slave_read(mldl_cfg, gyro_handle, compass_handle, - mldl_cfg->compass, &mldl_cfg->pdata->compass, - data); -} - -static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *pressure_handle, - unsigned char *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_slave_read(mldl_cfg, gyro_handle, pressure_handle, - mldl_cfg->pressure, - &mldl_cfg->pdata->pressure, data); -} - -/* Slave Config functions */ -int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *slave_handle, - struct ext_slave_config *data, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); -static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *accel_handle, - struct ext_slave_config *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_slave_config(mldl_cfg, gyro_handle, accel_handle, data, - mldl_cfg->accel, &mldl_cfg->pdata->accel); -} - -static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *compass_handle, - struct ext_slave_config *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_slave_config(mldl_cfg, gyro_handle, compass_handle, data, - mldl_cfg->compass, - &mldl_cfg->pdata->compass); -} - -static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *pressure_handle, - struct ext_slave_config *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_slave_config(mldl_cfg, gyro_handle, pressure_handle, - data, mldl_cfg->pressure, - &mldl_cfg->pdata->pressure); -} - -/* Slave get config functions */ -int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *slave_handle, - struct ext_slave_config *data, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - -static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *accel_handle, - struct ext_slave_config *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, accel_handle, - data, mldl_cfg->accel, - &mldl_cfg->pdata->accel); -} - -static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *compass_handle, - struct ext_slave_config *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, compass_handle, - data, mldl_cfg->compass, - &mldl_cfg->pdata->compass); -} - -static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *pressure_handle, - struct ext_slave_config *data) -{ - if (!mldl_cfg || !(mldl_cfg->pdata)) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, - pressure_handle, data, - mldl_cfg->pressure, - &mldl_cfg->pdata->pressure); -} - -/* -------------------------------------------------------------------------- */ - -static inline long inv_mpu_get_sampling_rate_hz(struct mldl_cfg *mldl_cfg) -{ - if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7)) - return 8000L / (mldl_cfg->divider + 1); - else - return 1000L / (mldl_cfg->divider + 1); -} - -static inline long inv_mpu_get_sampling_period_us(struct mldl_cfg *mldl_cfg) -{ - if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7)) - return (long) (1000000L * (mldl_cfg->divider + 1)) / 8000L; - else - return (long) (1000000L * (mldl_cfg->divider + 1)) / 1000L; -} - -#endif /* __MLDL_CFG_H__ */ - -/** - *@} - */ diff --git a/60xx/mlsdk/mllite/mldl_cfg_mpu.c b/60xx/mlsdk/mllite/mldl_cfg_mpu.c deleted file mode 100644 index 3d4496e..0000000 --- a/60xx/mlsdk/mllite/mldl_cfg_mpu.c +++ /dev/null @@ -1,477 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $ - * - *****************************************************************************/ - -/** - * @addtogroup MLDL - * - * @{ - * @file mldl_cfg_mpu.c - * @brief The Motion Library Driver Layer. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <stddef.h> -#include "mldl_cfg.h" -#include "mlsl.h" -#include "mpu.h" - -#ifdef LINUX -#include <sys/ioctl.h> -#endif - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mldl_cfg_mpu:" - -/* --------------------- */ -/* - Variables. - */ -/* --------------------- */ - - -/* ---------------------- */ -/* - Static Functions. - */ -/* ---------------------- */ -void mpu_print_cfg(struct mldl_cfg * mldl_cfg) -{ - struct mpu_platform_data *pdata = mldl_cfg->pdata; - struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel; - struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass; - struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure; - - MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr); - MPL_LOGD("mldl_cfg.int_config = %02x\n", mldl_cfg->int_config); - MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync); - MPL_LOGD("mldl_cfg.full_scale = %02x\n", mldl_cfg->full_scale); - MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf); - MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src); - MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider); - MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", mldl_cfg->dmp_enable); - MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", mldl_cfg->fifo_enable); - MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1); - MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2); - MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", mldl_cfg->offset_tc[0]); - MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", mldl_cfg->offset_tc[1]); - MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", mldl_cfg->offset_tc[2]); - MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision); - MPL_LOGD("mldl_cfg.product_id = %02x\n", mldl_cfg->product_id); - MPL_LOGD("mldl_cfg.gyro_sens_trim = %02x\n", mldl_cfg->gyro_sens_trim); -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 - MPL_LOGD("mldl_cfg.accel_sens_trim = %02x\n", mldl_cfg->accel_sens_trim); -#endif - - if (mldl_cfg->accel) { - MPL_LOGD("slave_accel->suspend = %p\n", mldl_cfg->accel->suspend); - MPL_LOGD("slave_accel->resume = %p\n", mldl_cfg->accel->resume); - MPL_LOGD("slave_accel->read = %p\n", mldl_cfg->accel->read); - MPL_LOGD("slave_accel->type = %02x\n", mldl_cfg->accel->type); - MPL_LOGD("slave_accel->read_reg = %02x\n", - mldl_cfg->accel->read_reg); - MPL_LOGD("slave_accel->read_len = %02x\n", - mldl_cfg->accel->read_len); - MPL_LOGD("slave_accel->endian = %02x\n", mldl_cfg->accel->endian); - MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa); - MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction); - } else { - MPL_LOGD("slave_accel = NULL\n"); - } - - if (mldl_cfg->compass) { - MPL_LOGD("slave_compass->suspend = %p\n", mldl_cfg->compass->suspend); - MPL_LOGD("slave_compass->resume = %p\n", mldl_cfg->compass->resume); - MPL_LOGD("slave_compass->read = %p\n", mldl_cfg->compass->read); - MPL_LOGD("slave_compass->type = %02x\n", mldl_cfg->compass->type); - MPL_LOGD("slave_compass->read_reg = %02x\n", - mldl_cfg->compass->read_reg); - MPL_LOGD("slave_compass->read_len = %02x\n", - mldl_cfg->compass->read_len); - MPL_LOGD("slave_compass->endian = %02x\n", mldl_cfg->compass->endian); - MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa); - MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction); - } else { - MPL_LOGD("slave_compass = NULL\n"); - } - - if (mldl_cfg->pressure) { - MPL_LOGD("slave_pressure->suspend = %p\n", mldl_cfg->pressure->suspend); - MPL_LOGD("slave_pressure->resume = %p\n", mldl_cfg->pressure->resume); - MPL_LOGD("slave_pressure->read = %p\n", mldl_cfg->pressure->read); - MPL_LOGD("slave_pressure->type = %02x\n", mldl_cfg->pressure->type); - MPL_LOGD("slave_pressure->read_reg = %02x\n", - mldl_cfg->pressure->read_reg); - MPL_LOGD("slave_pressure->read_len = %02x\n", - mldl_cfg->pressure->read_len); - MPL_LOGD("slave_pressure->endian = %02x\n", mldl_cfg->pressure->endian); - MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa); - MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction); - } else { - MPL_LOGD("slave_pressure = NULL\n"); - } - - MPL_LOGD("accel->get_slave_descr = %p\n",accel->get_slave_descr); - MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num); - MPL_LOGD("accel->bus = %02x\n", accel->bus); - MPL_LOGD("accel->address = %02x\n", accel->address); - MPL_LOGD("accel->orientation = \n" - " %2d %2d %2d\n" - " %2d %2d %2d\n" - " %2d %2d %2d\n", - accel->orientation[0],accel->orientation[1],accel->orientation[2], - accel->orientation[3],accel->orientation[4],accel->orientation[5], - accel->orientation[6],accel->orientation[7],accel->orientation[8]); - MPL_LOGD("compass->get_slave_descr = %p\n",compass->get_slave_descr); - MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num); - MPL_LOGD("compass->bus = %02x\n", compass->bus); - MPL_LOGD("compass->address = %02x\n", compass->address); - MPL_LOGD("compass->orientation = \n" - " %2d %2d %2d\n" - " %2d %2d %2d\n" - " %2d %2d %2d\n", - compass->orientation[0],compass->orientation[1],compass->orientation[2], - compass->orientation[3],compass->orientation[4],compass->orientation[5], - compass->orientation[6],compass->orientation[7],compass->orientation[8]); - MPL_LOGD("pressure->get_slave_descr = %p\n",pressure->get_slave_descr); - MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num); - MPL_LOGD("pressure->bus = %02x\n", pressure->bus); - MPL_LOGD("pressure->address = %02x\n", pressure->address); - MPL_LOGD("pressure->orientation = \n" - " %2d %2d %2d\n" - " %2d %2d %2d\n" - " %2d %2d %2d\n", - pressure->orientation[0],pressure->orientation[1],pressure->orientation[2], - pressure->orientation[3],pressure->orientation[4],pressure->orientation[5], - pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]); - - MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config); - MPL_LOGD("pdata->level_shifter = %02x\n", pdata->level_shifter); - MPL_LOGD("pdata->orientation = \n" - " %2d %2d %2d\n" - " %2d %2d %2d\n" - " %2d %2d %2d\n", - pdata->orientation[0],pdata->orientation[1],pdata->orientation[2], - pdata->orientation[3],pdata->orientation[4],pdata->orientation[5], - pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]); - - MPL_LOGD("Struct sizes: mldl_cfg: %zu, " - "ext_slave_descr:%zu, mpu_platform_data:%zu: RamOffset: %zu\n", - sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr), - sizeof(struct mpu_platform_data), - offsetof(struct mldl_cfg, ram)); -} - -/****************************************************************************** - ****************************************************************************** - * Exported functions - ****************************************************************************** - *****************************************************************************/ - -/** - * Initializes the pdata structure to defaults. - * - * Opens the device to read silicon revision, product id and whoami. Leaves - * the device in suspended state for low power. - * - * @param mldl_cfg handle to the config structure - * @param mlsl_handle handle to the mpu serial layer - * @param accel_handle handle to the accel serial layer - * @param compass_handle handle to the compass serial layer - * @param pressure_handle handle to the pressure serial layer - * - * @return INV_SUCCESS if silicon revision, product id and woami are supported - * by this software. - */ -int inv_mpu_open(struct mldl_cfg *mldl_cfg, - void *mlsl_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle) -{ - int result; - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL, - INV_ALL_SENSORS); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -/** - * Stub for driver close. Just verify that the devices are suspended - * - * @param mldl_cfg handle to the config structure - * @param mlsl_handle handle to the mpu serial layer - * @param accel_handle handle to the accel serial layer - * @param compass_handle handle to the compass serial layer - * @param pressure_handle handle to the compass serial layer - * - * @return INV_SUCCESS or non-zero error code - */ -int inv_mpu_close(struct mldl_cfg *mldl_cfg, - void *mlsl_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle) -{ - int result = INV_SUCCESS; - - result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL, - INV_ALL_SENSORS); - return result; -} - -int inv_mpu_resume(struct mldl_cfg* mldl_cfg, - void *mlsl_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle, - unsigned long sensors) -{ - int result; - - mldl_cfg->requested_sensors = sensors; - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_RESUME, NULL); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - //MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors); - - return result; -} - - -int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, - void *mlsl_handle, - void *accel_handle, - void *compass_handle, - void *pressure_handle, - unsigned long sensors) -{ - int result; - unsigned long requested = mldl_cfg->requested_sensors; - - mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS; - //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__, - // mldl_cfg->requested_sensors); - - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SUSPEND, NULL); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - mldl_cfg->requested_sensors = requested; - //MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested); - - return result; -} - -/** - * Send slave configuration information - * - * @param mldl_cfg pointer to the mldl configuration structure - * @param gyro_handle handle to the gyro sensor - * @param slave_handle handle to the slave sensor - * @param slave slave description - * @param pdata slave platform data - * @param data where to store the read data - * - * @return 0 or non-zero error code - */ -int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *slave_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - unsigned char *data) -{ - int result; - if (!mldl_cfg || !gyro_handle || !data || !slave) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - switch (slave->type) { - case EXT_SLAVE_TYPE_ACCELEROMETER: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_ACCEL, data); - break; - case EXT_SLAVE_TYPE_COMPASS: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_COMPASS, data); - break; - case EXT_SLAVE_TYPE_PRESSURE: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_PRESSURE, data); - break; - default: - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} - -/** - * Send slave configuration information - * - * @param mldl_cfg pointer to the mldl configuration structure - * @param gyro_handle handle to the gyro sensor - * @param slave_handle handle to the slave sensor - * @param data the data being sent - * @param slave slave description - * @param pdata slave platform data - * - * @return 0 or non-zero error code - */ -int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *slave_handle, - struct ext_slave_config *data, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata) -{ - int result; - if (!mldl_cfg || !data || !slave || !pdata || !slave) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - switch (slave->type) { - case EXT_SLAVE_TYPE_ACCELEROMETER: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_ACCEL, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case EXT_SLAVE_TYPE_COMPASS: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_COMPASS, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case EXT_SLAVE_TYPE_PRESSURE: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_PRESSURE, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - default: - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - break; - } - - return result; -} - -/** - * Request slave configuration information - * - * Use this specifically after requesting a slave configuration to see what the - * slave accually accepted. - * - * @param mldl_cfg pointer to the mldl configuration structure - * @param gyro_handle handle to the gyro sensor - * @param slave_handle handle to the slave sensor - * @param data the data being requested. - * @param slave slave description - * @param pdata slave platform data - * - * @return 0 or non-zero error code - */ -int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg, - void *gyro_handle, - void *slave_handle, - struct ext_slave_config *data, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata) -{ - int result; - if (!mldl_cfg || !data || !slave) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - switch (slave->type) { - case EXT_SLAVE_TYPE_ACCELEROMETER: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_ACCEL, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case EXT_SLAVE_TYPE_COMPASS: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_COMPASS, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - case EXT_SLAVE_TYPE_PRESSURE: - result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_PRESSURE, data); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - break; - default: - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - break; - } - return result; -} -/** - *@} - */ diff --git a/60xx/mlsdk/mllite/mldmp.c b/60xx/mlsdk/mllite/mldmp.c deleted file mode 100644 index 7381dd4..0000000 --- a/60xx/mlsdk/mllite/mldmp.c +++ /dev/null @@ -1,284 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mldmp.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *****************************************************************************/ - -/** - * @addtogroup MLDMP - * - * @{ - * @file mldmp.c - * @brief Shared functions between all the different DMP versions -**/ - -#include <stdio.h> - -#include "mltypes.h" -#include "mlinclude.h" -#include "mltypes.h" -#include "ml.h" -#include "mldl_cfg.h" -#include "mldl.h" -#include "compass.h" -#include "mlSetGyroBias.h" -#include "mlsl.h" -#include "mlFIFO.h" -#include "mldmp.h" -#include "mlstates.h" -#include "dmpDefault.h" -#include "mlFIFOHW.h" -#include "mlsupervisor.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-dmp" - -/** - * @brief Open the default motion sensor engine. - * This function is used to open the default MPL engine, - * featuring, for example, sensor fusion (6 axes and 9 axes), - * sensor calibration, accelerometer data byte swapping, among - * others. - * Compare with the other provided engines. - * - * @pre inv_serial_start() must have been called to instantiate the serial - * communication. - * - * Example: - * @code - * result = inv_dmp_open( ); - * if (INV_SUCCESS != result) { - * // Handle the error case - * } - * @endcode - * - * @return Zero on success; Error Code on any failure. - * - */ -inv_error_t inv_dmp_open(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - unsigned char state = inv_get_state(); - struct mldl_cfg *mldl_cfg; - unsigned long requested_sensors; - - /************************************************************* - * Common operations before calling DMPOpen - ************************************************************/ - if (state == INV_STATE_DMP_OPENED) - return INV_SUCCESS; - - if (state == INV_STATE_DMP_STARTED) { - return inv_dmp_stop(); - } - - result = inv_state_transition(INV_STATE_DMP_OPENED); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_dl_open(inv_get_serial_handle()); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#ifdef ML_USE_DMP_SIM - do { - void setup_univ(); - setup_univ(); /* hijack the read and write paths - and re-direct them to the simulator */ - } while (0); -#endif - - result = inv_setup_dmp(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - // Init vars. - inv_init_ml(); - - result = inv_init_fifo_param(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_set_bias(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - inv_init_fifo_hardare(); - mldl_cfg = inv_get_dl_config(); - requested_sensors = INV_THREE_AXIS_GYRO; - if (mldl_cfg->accel && mldl_cfg->accel->resume) - requested_sensors |= INV_THREE_AXIS_ACCEL; - - if (mldl_cfg->compass && mldl_cfg->compass->resume) - requested_sensors |= INV_THREE_AXIS_COMPASS; - - if (mldl_cfg->pressure && mldl_cfg->pressure->resume) - requested_sensors |= INV_THREE_AXIS_PRESSURE; - - result = inv_init_requested_sensors(requested_sensors); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_apply_calibration(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - if (NULL != mldl_cfg->accel){ - result = inv_apply_endian_accel(); - } - - return result; -} - -/** - * @brief Start the DMP. - * - * @pre inv_dmp_open() must have been called. - * - * @code - * result = inv_dmp_start(); - * if (INV_SUCCESS != result) { - * // Handle the error case - * } - * @endcode - * - * @return INV_SUCCESS if successful, or Non-zero error code otherwise. - */ -inv_error_t inv_dmp_start(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - if (inv_get_state() == INV_STATE_DMP_STARTED) - return INV_SUCCESS; - - result = inv_state_transition(INV_STATE_DMP_STARTED); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - inv_init_sensor_fusion_supervisor(); - result = inv_dl_start(inv_get_dl_config()->requested_sensors); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - /* This is done after the start since it will modify DMP memory, which - * will cause a full reset is most cases */ - result = inv_reset_motion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/** - * @brief Stops the DMP and puts it in low power. - * - * @pre inv_dmp_start() must have been called. - * - * @return INV_SUCCESS, Non-zero error code otherwise. - */ -inv_error_t inv_dmp_stop(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - - if (inv_get_state() == INV_STATE_DMP_OPENED) - return INV_SUCCESS; - - result = inv_state_transition(INV_STATE_DMP_OPENED); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_dl_stop(INV_ALL_SENSORS); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/** - * @brief Closes the motion sensor engine. - * Does not close the serial communication. To do that, - * call inv_serial_stop(). - * After calling inv_dmp_close() another DMP module can be - * loaded in the MPL with the corresponding necessary - * intialization and configurations, via any of the - * MLDmpXXXOpen functions. - * - * @pre inv_dmp_open() must have been called. - * - * @code - * result = inv_dmp_close(); - * if (INV_SUCCESS != result) { - * // Handle the error case - * } - * @endcode - * - * @return INV_SUCCESS, Non-zero error code otherwise. - */ -inv_error_t inv_dmp_close(void) -{ - INVENSENSE_FUNC_START; - inv_error_t result; - inv_error_t firstError = INV_SUCCESS; - - if (inv_get_state() <= INV_STATE_DMP_CLOSED) - return INV_SUCCESS; - - result = inv_disable_set_bias(); - ERROR_CHECK_FIRST(firstError, result); - - result = inv_dl_stop(INV_ALL_SENSORS); - ERROR_CHECK_FIRST(firstError, result); - - result = inv_close_fifo(); - ERROR_CHECK_FIRST(firstError, result); - - result = inv_dl_close(); - ERROR_CHECK_FIRST(firstError, result); - - result = inv_state_transition(INV_STATE_SERIAL_OPENED); - ERROR_CHECK_FIRST(firstError, result); - - return result; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/mldmp.h b/60xx/mlsdk/mllite/mldmp.h deleted file mode 100644 index f519798..0000000 --- a/60xx/mlsdk/mllite/mldmp.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/***************************************************************************** * - * $Id: mldmp.h 5629 2011-06-11 03:13:08Z mcaramello $ - ******************************************************************************/ - -/** - * @defgroup MLDMP - * @brief - * - * These are the top level functions that define how to load the MPL. In order - * to use most of the features, the DMP must be loaded with some code. The - * loading procedure takes place when calling inv_dmp_open with a given DMP set - * function, after having open the serial communication with the device via - * inv_serial_start(). - * The DMP set function will load the DMP memory and enable a certain - * set of features. - * - * First select a DMP version from one of the released DMP sets. - * These could be: - * - DMP default to load and use the default DMP code featuring pedometer, - * gestures, and orientation. Use inv_dmp_open(). - * - DMP pedometer stand-alone to load and use the standalone pedometer - * implementation. Use inv_open_low_power_pedometer(). - * <!-- - DMP EIS ... Use inv_eis_open_dmp(). --> - * - * After inv_dmp_openXXX any number of appropriate initialization and configuration - * routines can be called. Each one of these routines will return an error code - * and will check to make sure that it is compatible with the the DMP version - * selected during the call to inv_dmp_open. - * - * Once the configuration is complete, make a call to inv_dmp_start(). This will - * finally turn on the DMP and run the code previously loaded. - * - * While the DMP is running, all data fetching, polling or other functions can - * be called and will return valid data. Some parameteres can be changed while - * the DMP is runing, while others cannot. Therefore it is important to always - * check the return code of each function. Check the error code list in mltypes - * to know what each returned error corresponds to. - * - * When no more motion processing is required, the library can be shut down and - * the DMP turned off. We can do that by calling inv_dmp_close(). Note that - * inv_dmp_close() will not close the serial communication automatically, which will - * remain open an active, in case another module needs to be loaded instead. - * If the intention is shutting down the MPL as well, an explicit call to - * inv_serial_stop() following inv_dmp_close() has to be made. - * - * The MPL additionally implements a basic state machine, whose purpose is to - * give feedback to the user on whether he is following all the required - * initialization steps. If an anomalous transition is detected, the user will - * be warned by a terminal message with the format: - * - * <tt>"Error : illegal state transition from STATE_1 to STATE_3"</tt> - * - * @{ - * @file mldmp.h - * @brief Top level entry functions to the MPL library with DMP support - */ - -#ifndef MLDMP_H -#define MLDMP_H -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mldmp_legacy.h" -#endif - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_dmp_open(void); - inv_error_t inv_dmp_start(void); - inv_error_t inv_dmp_stop(void); - inv_error_t inv_dmp_close(void); - -#ifdef __cplusplus -} -#endif -#endif /* MLDMP_H */ -/** - * @} -**/ diff --git a/60xx/mlsdk/mllite/mlinclude.h b/60xx/mlsdk/mllite/mlinclude.h deleted file mode 100644 index dcbe904..0000000 --- a/60xx/mlsdk/mllite/mlinclude.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef INV_INCLUDE_H__ -#define INV_INCLUDE_H__ - -#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere - -#ifdef COVERAGE -#include "utestCommon.h" -#endif -#ifdef PROFILE -#include "profile.h" -#endif - -#ifdef WIN32 -#ifdef COVERAGE - -extern int functionEnterLog(const char *file, const char *func); -extern int functionExitLog(const char *file, const char *func); - -#undef INVENSENSE_FUNC_START -#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \ - int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__) -#endif // COVERAGE -#endif // WIN32 - -#ifdef PROFILE -#undef INVENSENSE_FUNC_START -#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__) -#define return if ( profileExit(__FILE__, __FUNCTION__) ) return -#endif // PROFILE - -// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return - -#endif //INV_INCLUDE_H__ diff --git a/60xx/mlsdk/mllite/mlstates.c b/60xx/mlsdk/mllite/mlstates.c deleted file mode 100644 index 8ebb086..0000000 --- a/60xx/mlsdk/mllite/mlstates.c +++ /dev/null @@ -1,269 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup MLSTATES - * @brief Basic state machine definition and support for the Motion Library. - * - * @{ - * @file mlstates.c - * @brief The Motion Library state machine definition. - */ - -#define ML_C - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <stdio.h> -#include <string.h> - -#include "mlstates.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "ml.h" -#include "mlos.h" - -#include <log.h> -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mlstates" - -#define _stateDebug(x) //{x} - -#define MAX_STATE_CHANGE_PROCESSES (8) - -struct state_callback_obj { - int_fast8_t numStateChangeCallbacks; - HANDLE mutex; - state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES]; -}; - -static struct state_callback_obj sStateChangeCallbacks = { 0 }; - -/* --------------- */ -/* - Functions. - */ -/* --------------- */ - -static inv_error_t inv_init_state_callbacks(void) -{ - memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks)); - return inv_create_mutex(&sStateChangeCallbacks.mutex); -} - -static inv_error_t MLStateCloseCallbacks(void) -{ - inv_error_t result; - result = inv_destroy_mutex(sStateChangeCallbacks.mutex); - memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks)); - return result; -} - -/** - * @internal - * @brief return a string containing the label assigned to the given state. - * @param state The state of which the label has to be returned. - * @return A string containing the state label. -**/ -char *inv_state_name(unsigned char state) -{ - switch (state) { - case INV_STATE_SERIAL_CLOSED: - return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED); - break; - case INV_STATE_SERIAL_OPENED: - return INV_STATE_NAME(INV_STATE_SERIAL_OPENED); - break; - case INV_STATE_DMP_OPENED: - return INV_STATE_NAME(INV_STATE_DMP_OPENED); - break; - case INV_STATE_DMP_STARTED: - return INV_STATE_NAME(INV_STATE_DMP_STARTED); - break; - default: - return NULL; - } -} - -/** - * @internal - * @brief Perform a transition from the current state to newState. - * Check for the correctness of the transition. - * Print out an error message if the transition is illegal . - * This routine is also called if a certain normally constant parameters - * are changed such as the FIFO Rate. - * @param newState state we are transitioning to. - * @return -**/ -inv_error_t inv_state_transition(unsigned char newState) -{ - inv_error_t result = INV_SUCCESS; - - if (newState == INV_STATE_SERIAL_CLOSED) { - // Always allow transition to closed - } else if (newState == INV_STATE_SERIAL_OPENED) { - inv_init_state_callbacks(); // Always allow first transition to start over - } else if (((newState == INV_STATE_DMP_OPENED) && - ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) || - (inv_params_obj.state == INV_STATE_DMP_STARTED))) - || - ((newState == INV_STATE_DMP_STARTED) && - (inv_params_obj.state == INV_STATE_DMP_OPENED))) { - // Valid transitions but no special action required - } else { - // All other combinations are illegal - MPL_LOGE("Error : illegal state transition from %s to %s\n", - inv_state_name(inv_params_obj.state), - inv_state_name(newState)); - result = INV_ERROR_SM_TRANSITION; - } - - if (result == INV_SUCCESS) { - _stateDebug(MPL_LOGV - ("ML State transition from %s to %s\n", - inv_state_name(inv_params_obj.state), - inv_state_name(newState))); - result = inv_run_state_callbacks(newState); - if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) { - MLStateCloseCallbacks(); - } - inv_params_obj.state = newState; - } - return result; -} - -/** - * @internal - * @brief To be moved in mlstates.c -**/ -unsigned char inv_get_state(void) -{ - return (inv_params_obj.state); -} - -/** - * @internal - * @brief This registers a function to be called each time the state - * changes. It may also be called when the FIFO Rate is changed. - * It will be called at the start of a state change before the - * state change has taken place. See Also inv_unregister_state_callback() - * The FIFO does not have to be on for this callback. - * @param func Function to be called when a DMP interrupt occurs. - * @return INV_SUCCESS or non-zero error code. - */ - -inv_error_t inv_register_state_callback(state_change_callback_t callback) -{ - INVENSENSE_FUNC_START; - int kk; - inv_error_t result; - - result = inv_lock_mutex(sStateChangeCallbacks.mutex); - if (INV_SUCCESS != result) { - return result; - } - // Make sure we have not filled up our number of allowable callbacks - if (sStateChangeCallbacks.numStateChangeCallbacks < - MAX_STATE_CHANGE_PROCESSES) { - // Make sure we haven't registered this function already - for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { - if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) { - result = INV_ERROR_INVALID_PARAMETER; - break; - } - } - - if (INV_SUCCESS == result) { - // Add new callback - sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks. - numStateChangeCallbacks] - = callback; - sStateChangeCallbacks.numStateChangeCallbacks++; - } - } else { - result = INV_ERROR_MEMORY_EXAUSTED; - } - - inv_unlock_mutex(sStateChangeCallbacks.mutex); - return result; -} - -/** - * @internal - * @brief This unregisters a function to be called each time the state - * changes. See Also inv_register_state_callback() - * The FIFO does not have to be on for this callback. - * @return INV_SUCCESS or non-zero error code. - */ -inv_error_t inv_unregister_state_callback(state_change_callback_t callback) -{ - INVENSENSE_FUNC_START; - int kk, jj; - inv_error_t result; - - result = inv_lock_mutex(sStateChangeCallbacks.mutex); - if (INV_SUCCESS != result) { - return result; - } - // Make sure we haven't registered this function already - result = INV_ERROR_INVALID_PARAMETER; - for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { - if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) { - for (jj = kk + 1; - jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) { - sStateChangeCallbacks.stateChangeCallbacks[jj - 1] = - sStateChangeCallbacks.stateChangeCallbacks[jj]; - } - sStateChangeCallbacks.numStateChangeCallbacks--; - result = INV_SUCCESS; - break; - } - } - - inv_unlock_mutex(sStateChangeCallbacks.mutex); - return result; -} - -inv_error_t inv_run_state_callbacks(unsigned char newState) -{ - int kk; - inv_error_t result; - - result = inv_lock_mutex(sStateChangeCallbacks.mutex); - if (INV_SUCCESS != result) { - MPL_LOGE("MLOsLockMutex returned %d\n", result); - return result; - } - - for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { - if (sStateChangeCallbacks.stateChangeCallbacks[kk]) { - result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState); - if (INV_SUCCESS != result) { - break; // Can't return, must release mutex - } - } - } - - inv_unlock_mutex(sStateChangeCallbacks.mutex); - return result; -} diff --git a/60xx/mlsdk/mllite/mlstates.h b/60xx/mlsdk/mllite/mlstates.h deleted file mode 100644 index cbaa610..0000000 --- a/60xx/mlsdk/mllite/mlstates.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mlstates.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#ifndef INV_STATES_H__ -#define INV_STATES_H__ - -#include "mltypes.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlstates_legacy.h" -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* See inv_state_transition for the transition mask */ -#define INV_STATE_SERIAL_CLOSED (0) -#define INV_STATE_SERIAL_OPENED (1) -#define INV_STATE_DMP_OPENED (2) -#define INV_STATE_DMP_STARTED (3) -#define INV_STATE_DMP_STOPPED (INV_STATE_DMP_OPENED) -#define INV_STATE_DMP_CLOSED (INV_STATE_SERIAL_OPENED) - -#define INV_STATE_NAME(x) (#x) - - typedef inv_error_t(*state_change_callback_t) (unsigned char newState); - - char *inv_state_name(unsigned char x); - inv_error_t inv_state_transition(unsigned char newState); - unsigned char inv_get_state(void); - inv_error_t inv_register_state_callback(state_change_callback_t callback); - inv_error_t inv_unregister_state_callback(state_change_callback_t callback); - inv_error_t inv_run_state_callbacks(unsigned char newState); - -#ifdef __cplusplus -} -#endif -#endif // INV_STATES_H__ diff --git a/60xx/mlsdk/mllite/mlsupervisor.c b/60xx/mlsdk/mllite/mlsupervisor.c deleted file mode 100644 index 139297f..0000000 --- a/60xx/mlsdk/mllite/mlsupervisor.c +++ /dev/null @@ -1,597 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $ - * - *****************************************************************************/ - -/** - * @defgroup ML_SUPERVISOR - * @brief Basic sensor fusion supervisor functionalities. - * - * @{ - * @file mlsupervisor.c - * @brief Basic sensor fusion supervisor functionalities. - */ - -#include "ml.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mltypes.h" -#include "mlinclude.h" -#include "compass.h" -#include "pressure.h" -#include "dmpKey.h" -#include "dmpDefault.h" -#include "mlstates.h" -#include "mlFIFO.h" -#include "mlFIFOHW.h" -#include "mlMathFunc.h" -#include "mlsupervisor.h" -#include "mlmath.h" - -#include "mlsl.h" -#include "mlos.h" - -#include <log.h> -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-sup" - -static unsigned long lastCompassTime = 0; -static unsigned long polltime = 0; -static unsigned long coiltime = 0; -static int accCount = 0; -static int compassCalStableCount = 0; -static int compassCalCount = 0; -static int coiltimerstart = 0; -static unsigned long disturbtime = 0; -static int disturbtimerstart = 0; - -static yas_filter_if_s f; -static yas_filter_handle_t handle; - -#define SUPERVISOR_DEBUG 0 - -struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 }; - -/** - * @brief This initializes all variables that should be reset on - */ -void inv_init_sensor_fusion_supervisor(void) -{ - lastCompassTime = 0; - polltime = 0; - inv_obj.acc_state = SF_STARTUP_SETTLE; - accCount = 0; - compassCalStableCount = 0; - compassCalCount = 0; - - yas_filter_init(&f); - f.init(&handle); - -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 - if (inv_compass_present()) { - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) { - (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT); - } - } -#endif - - if (ml_supervisor_cb.supervisor_reset_func != NULL) { - ml_supervisor_cb.supervisor_reset_func(); - } -} - -static int MLUpdateCompassCalibration3DOF(int command, long *data, - unsigned long deltaTime) -{ - INVENSENSE_FUNC_START; - int retValue = INV_SUCCESS; - static float m[10][10] = { {0} }; - float mInv[10][10] = { {0} }; - float mTmp[10][10] = { {0} }; - static float xTransY[4] = { 0 }; - float magSqr = 0; - float inpData[3] = { 0 }; - int i, j; - int a, b; - float d; - - switch (command) { - case CAL_ADD_DATA: - inpData[0] = (float)data[0]; - inpData[1] = (float)data[1]; - inpData[2] = (float)data[2]; - m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]); - m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]); - m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]); - m[0][3] += (-2 * inpData[0]); - m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]); - m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]); - m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]); - m[1][3] += (-2 * inpData[1]); - m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]); - m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]); - m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]); - m[2][3] += (-2 * inpData[2]); - m[3][0] += (-2 * inpData[0]); - m[3][1] += (-2 * inpData[1]); - m[3][2] += (-2 * inpData[2]); - m[3][3] += 1.0f; - magSqr = - inpData[0] * inpData[0] + inpData[1] * inpData[1] + - inpData[2] * inpData[2]; - xTransY[0] += (-2 * inpData[0]) * magSqr; - xTransY[1] += (-2 * inpData[1]) * magSqr; - xTransY[2] += (-2 * inpData[2]) * magSqr; - xTransY[3] += magSqr; - break; - case CAL_RUN: - a = 4; - b = a; - for (i = 0; i < b; i++) { - for (j = 0; j < b; j++) { - a = b; - inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j); - mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a); - } - } - a = b; - d = inv_matrix_det(&m[0][0], &a); - if (d == 0) { - return INV_ERROR; - } - for (i = 0; i < 3; i++) { - float tmp = 0; - for (j = 0; j < 4; j++) { - tmp += mInv[j][i] / d * xTransY[j]; - } - inv_obj.compass_test_bias[i] = - -(long)(tmp * inv_obj.compass_sens / 16384.0f); - } - break; - case CAL_RESET: - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - m[i][j] = 0; - } - xTransY[i] = 0; - } - default: - break; - } - return retValue; -} - -/** - * Entry point for Sensor Fusion operations - * @internal - * @param magFB magnetormeter FB - * @param accSF accelerometer SF - */ -static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF, - unsigned long deltaTime) -{ - static long prevCompassBias[3] = { 0 }; - static long magMax[3] = { - -1073741824L, - -1073741824L, - -1073741824L - }; - static long magMin[3] = { - 1073741824L, - 1073741824L, - 1073741824L - }; - int gyroMag; - long accMag; - inv_error_t result; - int i; - - if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) { - ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime); - } - - gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT; - accMag = inv_accel_sum_of_sqr(); - - // Scaling below assumes certain scaling. -#if ACC_MAG_SQR_SHIFT != 16 -#error -#endif - - if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) { - result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) { - //Most basic compass calibration, used only with lite MPL - if (inv_obj.resetting_compass == 1) { - for (i = 0; i < 3; i++) { - magMax[i] = -1073741824L; - magMin[i] = 1073741824L; - prevCompassBias[i] = 0; - } - compassCalStableCount = 0; - compassCalCount = 0; - inv_obj.resetting_compass = 0; - } - if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) { - if (compassCalStableCount < 1000) { - for (i = 0; i < 3; i++) { - if (inv_obj.compass_sensor_data[i] > magMax[i]) { - magMax[i] = inv_obj.compass_sensor_data[i]; - } - if (inv_obj.compass_sensor_data[i] < magMin[i]) { - magMin[i] = inv_obj.compass_sensor_data[i]; - } - } - MLUpdateCompassCalibration3DOF(CAL_ADD_DATA, - inv_obj.compass_sensor_data, - deltaTime); - compassCalStableCount += deltaTime; - for (i = 0; i < 3; i++) { - if (magMax[i] - magMin[i] < - (long long)40 * 1073741824 / inv_obj.compass_sens) { - compassCalStableCount = 0; - } - } - } else { - if (compassCalStableCount >= 1000) { - if (MLUpdateCompassCalibration3DOF - (CAL_RUN, inv_obj.compass_sensor_data, - deltaTime) == INV_SUCCESS) { - inv_obj.got_compass_bias = 1; - inv_obj.compass_accuracy = 3; - for(i=0; i<3; i++) { - inv_obj.compass_bias_error[i] = 35; - } - if (inv_obj.compass_state == SF_UNCALIBRATED) - inv_obj.compass_state = SF_STARTUP_SETTLE; - inv_set_compass_bias(inv_obj.compass_test_bias); - } - compassCalCount = 0; - compassCalStableCount = 0; - for (i = 0; i < 3; i++) { - magMax[i] = -1073741824L; - magMin[i] = 1073741824L; - prevCompassBias[i] = 0; - } - MLUpdateCompassCalibration3DOF(CAL_RESET, - inv_obj.compass_sensor_data, - deltaTime); - } - } - } - compassCalCount += deltaTime; - if (compassCalCount > 20000) { - compassCalCount = 0; - compassCalStableCount = 0; - for (i = 0; i < 3; i++) { - magMax[i] = -1073741824L; - magMin[i] = 1073741824L; - prevCompassBias[i] = 0; - } - MLUpdateCompassCalibration3DOF(CAL_RESET, - inv_obj.compass_sensor_data, - deltaTime); - } - } - - if (inv_obj.acc_state != SF_STARTUP_SETTLE) { - if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) { - inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing - accCount = 0; - } else { - if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214) - && ((inv_obj.acc_state == SF_DISTURBANCE) - || (inv_obj.acc_state == SF_SLOW_SETTLE))) { - accCount += deltaTime; - if (accCount > 0) { - inv_obj.acc_state = SF_FAST_SETTLE; - accCount = 0; - } - } else { - if (inv_obj.acc_state == SF_DISTURBANCE) { - accCount += deltaTime; - if (accCount > 500) { - inv_obj.acc_state = SF_SLOW_SETTLE; - accCount = 0; - } - } else if (inv_obj.acc_state == SF_SLOW_SETTLE) { - accCount += deltaTime; - if (accCount > 1000) { - inv_obj.acc_state = SF_NORMAL; - accCount = 0; - } - } else if (inv_obj.acc_state == SF_FAST_SETTLE) { - accCount += deltaTime; - if (accCount > 250) { - inv_obj.acc_state = SF_NORMAL; - accCount = 0; - } - } - } - } - } - if (inv_obj.acc_state == SF_STARTUP_SETTLE) { - accCount += deltaTime; - if (accCount > 50) { - *accSF = 1073741824; //Startup settling - inv_obj.acc_state = SF_NORMAL; - accCount = 0; - } - } else if ((inv_obj.acc_state == SF_NORMAL) - || (inv_obj.acc_state == SF_SLOW_SETTLE)) { - *accSF = inv_obj.accel_sens * 64; //Normal - } else if ((inv_obj.acc_state == SF_DISTURBANCE)) { - *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0) - } else if (inv_obj.acc_state == SF_FAST_SETTLE) { - *accSF = inv_obj.accel_sens * 512; //Amplify accel - } - if (!inv_get_gyro_present()) { - *accSF = inv_obj.accel_sens * 128; - } - return INV_SUCCESS; -} - -/** - * @brief Entry point for software sensor fusion operations. - * Manages hardware interaction, calls sensor fusion supervisor for - * bias calculation. - * @return error code. INV_SUCCESS if no error occurred. - */ -inv_error_t inv_accel_compass_supervisor(void) -{ - inv_error_t result; - int adjustSensorFusion = 0; - long accSF = 1073741824; - static double magFB = 0; - long magSensorData[3]; - float fcin[3]; - float fcout[3]; - - - if (inv_compass_present()) { /* check for compass data */ - int i, j; - long long tmp[3] = { 0 }; - long long tmp64 = 0; - unsigned long ctime = inv_get_tick_count(); - if (((inv_get_compass_id() == COMPASS_ID_AK8975) && - ((ctime - polltime) > 20)) || - (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max - if (SUPERVISOR_DEBUG) { - MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n"); - MPL_LOGV("delta time = %ld\n", ctime - polltime); - } - polltime = ctime; - result = inv_get_compass_data(magSensorData); - /* external slave wants the data even if there is an error */ - if (result && !inv_obj.external_slave_callback) { - if (SUPERVISOR_DEBUG) { - MPL_LOGW("inv_get_compass_data returned %d\n", result); - } - } else { - unsigned long compassTime = inv_get_tick_count(); - unsigned long deltaTime = 1; - - if (result == INV_SUCCESS) { - if (lastCompassTime != 0) { - deltaTime = compassTime - lastCompassTime; - } - lastCompassTime = compassTime; - adjustSensorFusion = 1; - if (inv_obj.got_init_compass_bias == 0) { - inv_obj.got_init_compass_bias = 1; - for (i = 0; i < 3; i++) { - inv_obj.init_compass_bias[i] = magSensorData[i]; - } - } - for (i = 0; i < 3; i++) { - inv_obj.compass_sensor_data[i] = (long)magSensorData[i]; - inv_obj.compass_sensor_data[i] -= - inv_obj.init_compass_bias[i]; - tmp[i] = ((long long)inv_obj.compass_sensor_data[i]) - * inv_obj.compass_sens / 16384; - tmp[i] -= inv_obj.compass_bias[i]; - tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L; - } - for (i = 0; i < 3; i++) { - tmp64 = 0; - for (j = 0; j < 3; j++) { - tmp64 += (long long) tmp[j] * - inv_obj.compass_cal[i * 3 + j]; - } - inv_obj.compass_calibrated_data[i] = - (long) (tmp64 / inv_obj.compass_sens); - } - //Additions: - if ((inv_obj.compass_state == 1) && - (inv_obj.compass_overunder == 0)) { - if (disturbtimerstart == 0) { - disturbtimerstart = 1; - disturbtime = ctime; - } - } else { - disturbtimerstart = 0; - } - - if (inv_obj.compass_overunder) { - if (coiltimerstart == 0) { - coiltimerstart = 1; - coiltime = ctime; - } - } - if (coiltimerstart == 1) { - if (ctime - coiltime > 3000) { - inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; - inv_set_compass_offset(); - inv_reset_compass_calibration(); - coiltimerstart = 0; - } - } - - if (disturbtimerstart == 1) { - if (ctime - disturbtime > 10000) { - inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; - inv_set_compass_offset(); - inv_reset_compass_calibration(); - MPL_LOGI("Compass reset! inv_reset_compass_calibration \n"); - disturbtimerstart = 0; - } - } - } - if (inv_obj.external_slave_callback) { - result = inv_obj.external_slave_callback(&inv_obj); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - -#ifdef APPLY_COMPASS_FILTER - if (inv_get_compass_id() == COMPASS_ID_YAS530) - { - fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); - fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); - fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); - - f.update(&handle, fcin, fcout); - - inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); - inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); - inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); - } -#endif - - if (SUPERVISOR_DEBUG) { - MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n", - (float)inv_obj.compass_calibrated_data[0] / - 65536.f, - (float)inv_obj.compass_calibrated_data[1] / - 65536.f, - (float)inv_obj.compass_calibrated_data[2] / - 65536.f); - } - magFB = 1.0; - adjustSensorFusion = 1; - result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - } - } else { - //No compass, but still modify accel - unsigned long ctime = inv_get_tick_count(); - if (polltime == 0 || ((ctime - polltime) > 80)) { // at the beginning AND every 1/8 second - unsigned long deltaTime = 1; - adjustSensorFusion = 1; - magFB = 0; - if (polltime != 0) { - deltaTime = ctime - polltime; - } - MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); - polltime = ctime; - } - } - if (adjustSensorFusion == 1) { - unsigned char regs[4]; - static int prevAccSF = 1; - - if (accSF != prevAccSF) { - regs[0] = (unsigned char)((accSF >> 24) & 0xff); - regs[1] = (unsigned char)((accSF >> 16) & 0xff); - regs[2] = (unsigned char)((accSF >> 8) & 0xff); - regs[3] = (unsigned char)(accSF & 0xff); - result = inv_set_mpu_memory(KEY_D_0_96, 4, regs); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - prevAccSF = accSF; - } - } - - if (ml_supervisor_cb.accel_compass_fusion_func != NULL) - ml_supervisor_cb.accel_compass_fusion_func(magFB); - - return INV_SUCCESS; -} - -/** - * @brief Entry point for software sensor fusion operations. - * Manages hardware interaction, calls sensor fusion supervisor for - * bias calculation. - * @return INV_SUCCESS or non-zero error code on error. - */ -inv_error_t inv_pressure_supervisor(void) -{ - long pressureSensorData[1]; - static unsigned long pressurePolltime = 0; - if (inv_pressure_present()) { /* check for pressure data */ - unsigned long ctime = inv_get_tick_count(); - if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second - if (SUPERVISOR_DEBUG) { - MPL_LOGV("Fetch pressure data\n"); - MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime); - } - pressurePolltime = ctime; - if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) { - inv_obj.pressure = pressureSensorData[0]; - } - } - } - return INV_SUCCESS; -} - -/** - * @brief Resets the magnetometer calibration algorithm. - * @return INV_SUCCESS if successful, or non-zero error code otherwise. - */ -inv_error_t inv_reset_compass_calibration(void) -{ - if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) { - if (ml_supervisor_cb.reset_advanced_compass_func != NULL) - ml_supervisor_cb.reset_advanced_compass_func(); - } - MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1); - - inv_obj.compass_bias_error[0] = P_INIT; - inv_obj.compass_bias_error[1] = P_INIT; - inv_obj.compass_bias_error[2] = P_INIT; - inv_obj.compass_accuracy = 0; - - inv_obj.got_compass_bias = 0; - inv_obj.got_init_compass_bias = 0; - inv_obj.compass_state = SF_UNCALIBRATED; - inv_obj.resetting_compass = 1; - - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/mlsupervisor.h b/60xx/mlsdk/mllite/mlsupervisor.h deleted file mode 100644 index 62b427e..0000000 --- a/60xx/mlsdk/mllite/mlsupervisor.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mlsupervisor.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *****************************************************************************/ - -#ifndef __INV_SUPERVISOR_H__ -#define __INV_SUPERVISOR_H__ - -#include "mltypes.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "mlsupervisor_legacy.h" -#endif - -// The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = 2^this_number -// this number must be >=0 and even. -#define GYRO_MAG_SQR_SHIFT 6 -// The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = 2^this_number -#define ACC_MAG_SQR_SHIFT 16 - -#define CAL_RUN 0 -#define CAL_RESET 1 -#define CAL_CHANGED_DATA 2 -#define CAL_RESET_TIME 3 -#define CAL_ADD_DATA 4 -#define CAL_COMBINE 5 - -#define P_INIT 100000 - -#define SF_NORMAL 0 -#define SF_DISTURBANCE 1 -#define SF_FAST_SETTLE 2 -#define SF_SLOW_SETTLE 3 -#define SF_STARTUP_SETTLE 4 -#define SF_UNCALIBRATED 5 - -struct inv_supervisor_cb_obj { - void (*accel_compass_fusion_func) (double magFB); - inv_error_t(*progressive_no_motion_supervisor_func) (unsigned long - deltaTime); - inv_error_t(*sensor_fusion_advanced_func) (double *magFB, - unsigned long deltaTime); - void (*reset_advanced_compass_func) (void); - void (*supervisor_reset_func) (void); -}; - -inv_error_t inv_reset_compass_calibration(void); -void inv_init_sensor_fusion_supervisor(void); -inv_error_t inv_accel_compass_supervisor(void); -inv_error_t inv_pressure_supervisor(void); - -#endif // __INV_SUPERVISOR_H__ - diff --git a/60xx/mlsdk/mllite/pressure.c b/60xx/mlsdk/mllite/pressure.c deleted file mode 100644 index 9c162ec..0000000 --- a/60xx/mlsdk/mllite/pressure.c +++ /dev/null @@ -1,166 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: pressure.c 4120 2010-11-21 19:56:16Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup PRESSUREDL - * @brief Motion Library - Pressure Driver Layer. - * Provides the interface to setup and handle a pressure sensor - * connected to either the primary or the seconday I2C interface - * of the gyroscope. - * - * @{ - * @file pressure.c - * @brief Pressure setup and handling methods. -**/ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -#include <string.h> - -#include "pressure.h" - -#include "ml.h" -#include "mlinclude.h" -#include "dmpKey.h" -#include "mlFIFO.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "mlMathFunc.h" -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-pressure" - -#define _pressureDebug(x) //{x} - -/* --------------------- */ -/* - Global Variables. - */ -/* --------------------- */ - -/* --------------------- */ -/* - Static Variables. - */ -/* --------------------- */ - -/* --------------- */ -/* - Prototypes. - */ -/* --------------- */ - -/* -------------- */ -/* - Externs. - */ -/* -------------- */ - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Is a pressure configured and used by MPL? - * @return INV_SUCCESS if the pressure is present. - */ -unsigned char inv_pressure_present(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pressure && - NULL != mldl_cfg->pressure->resume && - mldl_cfg->requested_sensors & INV_THREE_AXIS_PRESSURE) - return TRUE; - else - return FALSE; -} - -/** - * @brief Query the pressure slave address. - * @return The 7-bit pressure slave address. - */ -unsigned char inv_get_pressure_slave_addr(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pdata) - return mldl_cfg->pdata->pressure.address; - else - return 0; -} - -/** - * @brief Get the ID of the pressure in use. - * @return ID of the pressure in use. - */ -unsigned short inv_get_pressure_id(void) -{ - INVENSENSE_FUNC_START; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - if (NULL != mldl_cfg->pressure) { - return mldl_cfg->pressure->id; - } - return ID_INVALID; -} - -/** - * @brief Get a sample of pressure data from the device. - * @param data - * the buffer to store the pressure raw data for - * X, Y, and Z axes. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t inv_get_pressure_data(long *data) -{ - inv_error_t result = INV_SUCCESS; - unsigned char tmp[3]; - struct mldl_cfg *mldl_cfg = inv_get_dl_config(); - - /*--- read the pressure sensor data. - The pressure read function may return an INV_ERROR_PRESSURE_* errors - when the data is not ready (read/refresh frequency mismatch) or - the internal data sampling timing of the device was not respected. - Returning the error code will make the sensor fusion supervisor - ignore this pressure data sample. ---*/ - result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg, - inv_get_serial_handle(), - inv_get_serial_handle(), tmp); - if (result) { - _pressureDebug(MPL_LOGV - ("inv_mpu_read_pressure returned %d (%s)\n", result, - MLErrorCode(result))); - return result; - } - if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->pressure->endian) - data[0] = - (((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) + - ((long)tmp[2]); - else - data[0] = - (((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) + - ((long)tmp[0]); - - return INV_SUCCESS; -} - -/** - * @} - */ diff --git a/60xx/mlsdk/mllite/pressure.h b/60xx/mlsdk/mllite/pressure.h deleted file mode 100644 index 77c5d43..0000000 --- a/60xx/mlsdk/mllite/pressure.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: pressure.h 4092 2010-11-17 23:49:22Z kkeal $ - * - *******************************************************************************/ - -#ifndef PRESSURE_H -#define PRESSURE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "mpu.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "pressure_legacy.h" -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -#define USE_PRESSURE_BMA 0 - -#define PRESSURE_SLAVEADDR_INVALID 0x00 -#define PRESSURE_SLAVEADDR_BMA085 0x77 - -/* - Define default pressure to use if no selection is made -*/ -#if USE_PRESSURE_BMA -#define DEFAULT_PRESSURE_TYPE PRESSURE_ID_BMA -#endif - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - - unsigned char inv_pressure_present(void); - unsigned char inv_get_pressure_slave_addr(void); - inv_error_t inv_suspend_pressure(void); - inv_error_t inv_resume_presure(void); - inv_error_t inv_get_pressure_data(long *data); - unsigned short inv_get_pressure_id(void); - -#ifdef __cplusplus -} -#endif -#endif // PRESSURE_H diff --git a/60xx/mlsdk/mlutils/checksum.c b/60xx/mlsdk/mlutils/checksum.c deleted file mode 100644 index a97477d..0000000 --- a/60xx/mlsdk/mlutils/checksum.c +++ /dev/null @@ -1,16 +0,0 @@ -#include "mltypes.h" - -/** bernstein hash, from public domain source */ - -uint32_t inv_checksum(unsigned char *str, int len) -{ - uint32_t hash = 5381; - int i, c; - - for (i = 0; i < len; i++) { - c = *(str + i); - hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ - } - - return hash; -} diff --git a/60xx/mlsdk/mlutils/checksum.h b/60xx/mlsdk/mlutils/checksum.h deleted file mode 100644 index 4d3f046..0000000 --- a/60xx/mlsdk/mlutils/checksum.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef MPLCHECKSUM_H -#define MPLCHECKSUM_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#ifdef INV_INCLUDE_LEGACY_HEADERS -#include "checksum_legacy.h" -#endif - - uint32_t inv_checksum(unsigned char *str, int len); - -#ifdef __cplusplus -} -#endif -#endif /* MPLCHECKSUM_H */ diff --git a/60xx/mlsdk/mlutils/mputest.c b/60xx/mlsdk/mlutils/mputest.c deleted file mode 100644 index ac0fa30..0000000 --- a/60xx/mlsdk/mlutils/mputest.c +++ /dev/null @@ -1,1396 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $ - * - *****************************************************************************/ - -/* - * MPU Self Test functions - * Version 2.4 - * May 13th, 2011 - */ - -/** - * @defgroup MPU_SELF_TEST - * @brief MPU Self Test functions - * - * These functions provide an in-site test of the MPU 3xxx chips. The main - * entry point is the inv_mpu_test function. - * This runs the tests (as described in the accompanying documentation) and - * writes a configuration file containing initial calibration data. - * inv_mpu_test returns INV_SUCCESS if the chip passes the tests. - * Otherwise, an error code is returned. - * The functions in this file rely on MLSL and MLOS: refer to the MPL - * documentation for more information regarding the system interface - * files. - * - * @{ - * @file mputest.c - * @brief MPU Self Test routines for assessing gyro sensor status - * after surface mount has happened on the target host platform. - */ - -#include <stdio.h> -#include <time.h> -#include <string.h> -#include <math.h> -#include <stdlib.h> -#ifdef LINUX -#include <unistd.h> -#endif - -#include "mpu.h" -#include "mldl.h" -#include "mldl_cfg.h" -#include "accel.h" -#include "mlFIFO.h" -#include "slave.h" -#include "ml.h" -#include "ml_stored_data.h" -#include "checksum.h" - -#include "mlsl.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mpust" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Defines -*/ - -#define VERBOSE_OUT 0 - -/*#define TRACK_IDS*/ - -/*--- Test parameters defaults. See set_test_parameters for more details ---*/ - -#define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */ - -#if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */ -#define DEF_GYRO_FULLSCALE (2000) -#else -#define DEF_GYRO_FULLSCALE (250) -#endif - -#define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE) - /* gyro sensitivity LSB/dps */ -#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ -#define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */ -#define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS) - /* 40 dps in LSBs */ -#define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS) - /* 0.4 dps-rms in LSB-rms */ -#define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */ -#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting - data for each axis, - multiple of 600ms */ -#define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to - average from, if applic. */ - -#define ML_INIT_CAL_LEN (36) /* length in bytes of - calibration data file */ - -/* - Macros -*/ - -#define CHECK_TEST_ERROR(x) \ - if (x) { \ - MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \ - return (-1); \ - } - -#define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f) - -#define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \ - (chr)[1]=(unsigned char)(shrt); - -#define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \ - (chr)[1]=(unsigned char)(ui>>16); \ - (chr)[2]=(unsigned char)(ui>>8); \ - (chr)[3]=(unsigned char)(ui); - -#define FLOAT_TO_SHORT(f) ( \ - (fabs(f-(short)f)>=0.5) ? ( \ - ((short)f)+(f<0?(-1):(+1))) : \ - ((short)f) \ - ) - -#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) -#define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0]) - -#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5] - -#define CHECK_NACKS(d) ( \ - d[0]==0xff && d[1]==0xff && \ - d[2]==0xff && d[3]==0xff && \ - d[4]==0xff && d[5]==0xff \ - ) - -/* - Prototypes -*/ - -static inv_error_t test_get_data( - void *mlsl_handle, - struct mldl_cfg *mputestCfgPtr, - short *vals); - -/* - Types -*/ -typedef struct { - float gyro_sens; - int gyro_fs; - int packet_thresh; - float total_timing_tol; - int bias_thresh; - float rms_threshSq; - float sp_shift_thresh; - unsigned int test_time_per_axis; - unsigned short accel_samples; -} tTestSetup; - -/* - Global variables -*/ -static unsigned char dataout[20]; -static unsigned char dataStore[ML_INIT_CAL_LEN]; - -static tTestSetup test_setup = { - DEF_GYRO_SENS, - DEF_GYRO_FULLSCALE, - DEF_PACKET_THRESH, - DEF_TOTAL_TIMING_TOL, - (int)DEF_BIAS_THRESH, - DEF_RMS_THRESH * DEF_RMS_THRESH, - DEF_SP_SHIFT_THRESH_CUST, - DEF_TEST_TIME_PER_AXIS, - DEF_N_ACCEL_SAMPLES -}; - -static float adjGyroSens; -static char a_name[3][2] = {"X", "Y", "Z"}; - -/* - NOTE : modify get_slave_descr parameter below to reflect - the DEFAULT accelerometer in use. The accelerometer in use - can be modified at run-time using the inv_test_setup_accel API. - NOTE : modify the expected z axis orientation (Z axis pointing - upward or downward) -*/ - -signed char g_z_sign = +1; -struct mldl_cfg *mputestCfgPtr = NULL; - -#ifndef LINUX -/** - * @internal - * @brief usec precision sleep function. - * @param number of micro seconds (us) to sleep for. - */ -static void usleep(unsigned long t) -{ - unsigned long start = inv_get_tick_count(); - while (inv_get_tick_count()-start < t / 1000); -} -#endif - -/** - * @brief Modify the self test limits from their default values. - * - * @param slave_addr - * the slave address the MPU device is setup to respond at. - * The default is DEF_MPU_ADDR = 0x68. - * @param sensitivity - * the read sensitivity of the device in LSB/dps as it is trimmed. - * NOTE : if using the self test as part of the MPL, the - * sensitivity the different sensitivity trims are already - * taken care of. - * @param p_thresh - * number of packets expected to be received in a 600 ms period. - * Depends on the sampling frequency of choice (set by default to - * 125 Hz) and low pass filter cut-off frequency selection (set - * to 42 Hz). - * The default is DEF_PACKET_THRESH = 75 packets. - * @param total_time_tol - * time skew tolerance, taking into account imprecision in turning - * the FIFO on and off and the processor time imprecision (for - * 1 GHz processor). - * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets. - * @param bias_thresh - * bias level threshold, the maximun acceptable no motion bias - * for a production quality part. - * The default is DEF_BIAS_THRESH = 40 dps. - * @param rms_thresh - * the limit standard deviation (=~ RMS) set to assess whether - * the noise level on the part is acceptable. - * The default is DEF_RMS_THRESH = 0.2 dps-rms. - * @param sp_shift_thresh - * the limit shift applicable to the Sense Path self test - * calculation. - */ -void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, - int p_thresh, float total_time_tol, - int bias_thresh, float rms_thresh, - float sp_shift_thresh, - unsigned short accel_samples) -{ - mputestCfgPtr->addr = slave_addr; - test_setup.gyro_sens = sensitivity; - test_setup.gyro_fs = (int)(32768.f / sensitivity); - test_setup.packet_thresh = p_thresh; - test_setup.total_timing_tol = total_time_tol; - test_setup.bias_thresh = bias_thresh; - test_setup.rms_threshSq = rms_thresh * rms_thresh; - test_setup.sp_shift_thresh = sp_shift_thresh; - test_setup.accel_samples = accel_samples; -} - -#define X (0) -#define Y (1) -#define Z (2) - -#ifdef CONFIG_MPU_SENSORS_MPU3050 -/** - * @brief Test the gyroscope sensor. - * Implements the core logic of the MPU Self Test. - * Produces the PASS/FAIL result. Loads the calculated gyro biases - * and temperature datum into the corresponding pointers. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param gyro_biases - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements for gyro X, Y, - * and Z. - * @param temp_avg - * output pointer to store the initial average temperature as - * provided by the MPU Self Test. - * @param perform_full_test - * If 1: - * calculates offset, drive frequency, and noise and compare it - * against set thresholds. - * Report also the final result using a bit-mask like error code - * as explained in return value description. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment; simply calculates the gyro biases. - * - * @return 0 on success. - * On error, the return value is a bitmask representing: - * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively - * (decimal values will be 1, 2, 4 respectively). - * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively - * (decimal values will be 8, 16, 32 respectively). - * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively - * (decimal values will be 64, 128, 256 respectively). - * 9 If any of the RMS noise values is zero, it could be - * due to a non-functional gyro or FIFO/register failure. - * (decimal value will be 512). - * (decimal values will be 1024, 2048, 4096 respectively). - */ -int inv_test_gyro_3050(void *mlsl_handle, - short gyro_biases[3], short *temp_avg, - uint_fast8_t perform_full_test) -{ - int retVal = 0; - inv_error_t result; - - int total_count = 0; - int total_count_axis[3] = {0, 0, 0}; - int packet_count; - short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - unsigned char regs[7]; - - int temperature; - float Avg[3]; - float RMS[3]; - int i, j, tmp; - char tmpStr[200]; - - temperature = 0; - - /* sample rate = 8ms */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_SMPLRT_DIV, 0x07); - CHECK_TEST_ERROR(result); - - regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ - switch (DEF_GYRO_FULLSCALE) { - case 2000: - regs[0] |= 0x18; - break; - case 1000: - regs[0] |= 0x10; - break; - case 500: - regs[0] |= 0x08; - break; - case 250: - default: - regs[0] |= 0x00; - break; - } - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_DLPF_FS_SYNC, regs[0]); - CHECK_TEST_ERROR(result); - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_INT_CFG, 0x00); - CHECK_TEST_ERROR(result); - - /* 1st, timing test */ - for (j = 0; j < 3; j++) { - - MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); - - /* turn on all gyros, use gyro X for clocking - Set to Y and Z for 2nd and 3rd iteration */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, j + 1); - CHECK_TEST_ERROR(result); - - /* wait for 2 ms after switching clock source */ - usleep(2000); - - /* we will enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN2, 0x00); - CHECK_TEST_ERROR(result); - /* enable/reset FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); - CHECK_TEST_ERROR(result); - - tmp = (int)(test_setup.test_time_per_axis / 600); - while (tmp-- > 0) { - /* enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write(mlsl_handle, - mputestCfgPtr->addr, MPUREG_FIFO_EN1, - BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); - CHECK_TEST_ERROR(result); - - /* wait for 600 ms for data */ - usleep(600000); - - /* stop storing gyro in the FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN1, 0x00); - CHECK_TEST_ERROR(result); - - /* Getting number of bytes in FIFO */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_COUNTH, 2, dataout); - CHECK_TEST_ERROR(result); - /* number of 6 B packets in the FIFO */ - packet_count = CHARS_TO_SHORT(dataout) / 6; - sprintf(tmpStr, "Packet Count: %d - ", packet_count); - - if ( abs(packet_count - test_setup.packet_thresh) - <= /* Within +/- total_timing_tol % range */ - test_setup.total_timing_tol * test_setup.packet_thresh) { - for (i = 0; i < packet_count; i++) { - /* getting FIFO data */ - result = inv_serial_read_fifo(mlsl_handle, - mputestCfgPtr->addr, 6, dataout); - CHECK_TEST_ERROR(result); - x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); - y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); - z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); - if (VERBOSE_OUT) { - MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", - total_count + i, x[total_count + i], - y[total_count + i], z[total_count + i]); - } - } - total_count += packet_count; - total_count_axis[j] += packet_count; - sprintf(tmpStr, "%sOK", tmpStr); - } else { - if (perform_full_test) - retVal |= 1 << j; - sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); - } - MPL_LOGI("%s\n", tmpStr); - } - - /* remove gyros from FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN1, 0x00); - CHECK_TEST_ERROR(result); - - /* Read Temperature */ - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, - MPUREG_TEMP_OUT_H, 2, dataout); - CHECK_TEST_ERROR(result); - temperature += (short)CHARS_TO_SHORT(dataout); - } - - MPL_LOGI("\n"); - MPL_LOGI("Total %d samples\n", total_count); - MPL_LOGI("\n"); - - /* 2nd, check bias from X and Y PLL clock source */ - tmp = total_count != 0 ? total_count : 1; - for (i = 0, - Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; - i < total_count; i++) { - Avg[X] += 1.f * x[i] / tmp; - Avg[Y] += 1.f * y[i] / tmp; - Avg[Z] += 1.f * z[i] / tmp; - } - MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", - Avg[X], Avg[Y], Avg[Z]); - if (VERBOSE_OUT) { - MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", - Avg[X] / adjGyroSens, - Avg[Y] / adjGyroSens, - Avg[Z] / adjGyroSens); - } - if(perform_full_test) { - for (j = 0; j < 3; j++) { - if (fabs(Avg[j]) > test_setup.bias_thresh) { - MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " - "(threshold = %d)\n", - a_name[j], Avg[j], test_setup.bias_thresh); - retVal |= 1 << (3+j); - } - } - } - - /* 3rd, check RMS */ - if (perform_full_test) { - for (i = 0, - RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; - i < total_count; i++) { - RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); - RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); - RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); - } - for (j = 0; j < 3; j++) { - if (RMS[j] > test_setup.rms_threshSq * total_count) { - MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold " - "(threshold = %.2f)\n", - a_name[j], sqrt(RMS[j] / total_count), - sqrt(test_setup.rms_threshSq)); - retVal |= 1 << (6+j); - } - } - - MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", - sqrt(RMS[X] / total_count), - sqrt(RMS[Y] / total_count), - sqrt(RMS[Z] / total_count)); - if (VERBOSE_OUT) { - MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n", - RMS[X] / total_count, - RMS[Y] / total_count, - RMS[Z] / total_count); - } - - if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { - /* If any of the RMS noise value returns zero, - then we might have dead gyro or FIFO/register failure, - the part is sleeping, or the part is not responsive */ - retVal |= 1 << 9; - } - } - - /* 4th, temperature average */ - temperature /= 3; - if (VERBOSE_OUT) - MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", - SHORT_TO_TEMP_C(temperature), "", ""); - - /* load into final storage */ - *temp_avg = (short)temperature; - gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); - gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); - gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); - - return retVal; -} - -#else /* CONFIG_MPU_SENSORS_MPU3050 */ - -/** - * @brief Test the gyroscope sensor. - * Implements the core logic of the MPU Self Test but does not provide - * a PASS/FAIL output as in the MPU-3050 implementation. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param gyro_biases - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements for gyro X, Y, - * and Z. - * @param temp_avg - * output pointer to store the initial average temperature as - * provided by the MPU Self Test. - * - * @return 0 on success. - * A non-zero error code on error. - */ -int inv_test_gyro_6050(void *mlsl_handle, - short gyro_biases[3], short *temp_avg) -{ - inv_error_t result; - - int total_count = 0; - int total_count_axis[3] = {0, 0, 0}; - int packet_count; - short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; - unsigned char regs[7]; - - int temperature = 0; - float Avg[3]; - int i, j, tmp; - char tmpStr[200]; - - /* sample rate = 8ms */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_SMPLRT_DIV, 0x07); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ - switch (DEF_GYRO_FULLSCALE) { - case 2000: - regs[0] |= 0x18; - break; - case 1000: - regs[0] |= 0x10; - break; - case 500: - regs[0] |= 0x08; - break; - case 250: - default: - regs[0] |= 0x00; - break; - } - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_CONFIG, regs[0]); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_INT_ENABLE, 0x00); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* 1st, timing test */ - for (j = 0; j < 3; j++) { - MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); - - /* turn on all gyros, use gyro X for clocking - Set to Y and Z for 2nd and 3rd iteration */ -#ifdef CONFIG_MPU_SENSORS_MPU6050A2 - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1)); -#else - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGMT_1, j + 1); -#endif - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* wait for 2 ms after switching clock source */ - usleep(2000); - - /* enable/reset FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - tmp = (int)(test_setup.test_time_per_axis / 600); - while (tmp-- > 0) { - /* enable XYZ gyro in FIFO and nothing else */ - result = inv_serial_single_write(mlsl_handle, - mputestCfgPtr->addr, MPUREG_FIFO_EN, - BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* wait for 600 ms for data */ - usleep(600000); - /* stop storing gyro in the FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN, 0x00); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - /* Getting number of bytes in FIFO */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_COUNTH, 2, dataout); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - /* number of 6 B packets in the FIFO */ - packet_count = CHARS_TO_SHORT(dataout) / 6; - sprintf(tmpStr, "Packet Count: %d - ", packet_count); - - if (abs(packet_count - test_setup.packet_thresh) - <= /* Within +/- total_timing_tol % range */ - test_setup.total_timing_tol * test_setup.packet_thresh) { - for (i = 0; i < packet_count; i++) { - /* getting FIFO data */ - result = inv_serial_read_fifo(mlsl_handle, - mputestCfgPtr->addr, 6, dataout); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); - y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); - z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); - if (VERBOSE_OUT) { - MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", - total_count + i, x[total_count + i], - y[total_count + i], z[total_count + i]); - } - } - total_count += packet_count; - total_count_axis[j] += packet_count; - sprintf(tmpStr, "%sOK", tmpStr); - } else { - sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); - } - MPL_LOGI("%s\n", tmpStr); - } - - /* remove gyros from FIFO */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_FIFO_EN, 0x00); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* Read Temperature */ - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, - MPUREG_TEMP_OUT_H, 2, dataout); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - temperature += (short)CHARS_TO_SHORT(dataout); - } - - MPL_LOGI("\n"); - MPL_LOGI("Total %d samples\n", total_count); - MPL_LOGI("\n"); - - /* 2nd, check bias from X and Y PLL clock source */ - tmp = total_count != 0 ? total_count : 1; - for (i = 0, - Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; - i < total_count; i++) { - Avg[X] += 1.f * x[i] / tmp; - Avg[Y] += 1.f * y[i] / tmp; - Avg[Z] += 1.f * z[i] / tmp; - } - MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", - Avg[X], Avg[Y], Avg[Z]); - if (VERBOSE_OUT) { - MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", - Avg[X] / adjGyroSens, - Avg[Y] / adjGyroSens, - Avg[Z] / adjGyroSens); - } - - temperature /= 3; - if (VERBOSE_OUT) - MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", - SHORT_TO_TEMP_C(temperature), "", ""); - - /* load into final storage */ - *temp_avg = (short)temperature; - gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); - gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); - gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); - - return INV_SUCCESS; -} - -#endif /* CONFIG_MPU_SENSORS_MPU3050 */ - -#ifdef TRACK_IDS -/** - * @internal - * @brief Retrieve the unique MPU device identifier from the internal OTP - * bank 0 memory. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @return 0 on success, a non-zero error code from the serial layer on error. - */ -static inv_error_t test_get_mpu_id(void *mlsl_handle) -{ - inv_error_t result; - unsigned char otp0[8]; - - - result = - inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr, - (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 | - 0x00, 6, otp0); - if (result) - goto close; - - MPL_LOGI("\n"); - MPL_LOGI("DIE_ID : %06X\n", - ((int)otp0[1] << 8 | otp0[0]) & 0x1fff); - MPL_LOGI("WAFER_ID : %06X\n", - (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5); - MPL_LOGI("A_LOT_ID : %06X\n", - ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 | - otp0[2]) & 0x3ffff) >> 2); - MPL_LOGI("W_LOT_ID : %06X\n", - ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2); - MPL_LOGI("WP_ID : %06X\n", - ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7); - MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2); - MPL_LOGI("\n"); - -close: - result = - inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00); - return result; -} -#endif /* TRACK_IDS */ - -/** - * @brief If requested via inv_test_setup_accel(), test the accelerometer biases - * and calculate the necessary bias correction. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param bias - * output pointer to store the initial bias calculation provided - * by the MPU Self Test. Requires 3 elements to store accel X, Y, - * and Z axis bias. - * @param perform_full_test - * If 1: - * calculates offsets and noise and compare it against set - * thresholds. The final exist status will reflect if any of the - * value is outside of the expected range. - * When 0; - * skip the noise calculation and pass/fail assessment; simply - * calculates the accel biases. - * - * @return 0 on success. A non-zero error code on error. - */ -int inv_test_accel(void *mlsl_handle, - short *bias, long gravity, - uint_fast8_t perform_full_test) -{ - int i; - - short *p_vals; - float x = 0.f, y = 0.f, z = 0.f, zg = 0.f; - float RMS[3]; - float accelRmsThresh = 1000000.f; /* enourmous so that the test always - passes - future deployment */ - unsigned short res; - unsigned long orig_requested_sensors; - struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata; - - p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); - - /* load the slave descr from the getter */ - if (mputestPData->accel.get_slave_descr == NULL) { - MPL_LOGI("\n"); - MPL_LOGI("No accelerometer configured\n"); - return 0; - } - if (mputestCfgPtr->accel == NULL) { - MPL_LOGI("\n"); - MPL_LOGI("No accelerometer configured\n"); - return 0; - } - - /* resume the accel */ - orig_requested_sensors = mputestCfgPtr->requested_sensors; - mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO; - res = inv_mpu_resume(mputestCfgPtr, - mlsl_handle, NULL, NULL, NULL, - mputestCfgPtr->requested_sensors); - if(res != INV_SUCCESS) - goto accel_error; - - /* wait at least a sample cycle for the - accel data to be retrieved by MPU */ - inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000); - - /* collect the samples */ - for(i = 0; i < test_setup.accel_samples; i++) { - short *vals = &p_vals[3 * i]; - if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) { - goto accel_error; - } - x += 1.f * vals[X] / test_setup.accel_samples; - y += 1.f * vals[Y] / test_setup.accel_samples; - z += 1.f * vals[Z] / test_setup.accel_samples; - } - - mputestCfgPtr->requested_sensors = orig_requested_sensors; - res = inv_mpu_suspend(mputestCfgPtr, - mlsl_handle, NULL, NULL, NULL, - INV_ALL_SENSORS); - if (res != INV_SUCCESS) - goto accel_error; - - MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z); - if (VERBOSE_OUT) { - MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", - x / gravity, y / gravity, z / gravity); - } - - bias[0] = FLOAT_TO_SHORT(x); - bias[1] = FLOAT_TO_SHORT(y); - zg = z - g_z_sign * gravity; - bias[2] = FLOAT_TO_SHORT(zg); - - MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", - bias[0], bias[1], bias[2]); - if (VERBOSE_OUT) { - MPL_LOGI("Accel correct.: " - "%+13.3f %+13.3f %+13.3f (gee)\n", - 1.f * bias[0] / gravity, - 1.f * bias[1] / gravity, - 1.f * bias[2] / gravity); - } - - if (perform_full_test) { - /* accel RMS - for now the threshold is only indicative */ - for (i = 0, - RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; - i < test_setup.accel_samples; i++) { - short *vals = &p_vals[3 * i]; - RMS[X] += (vals[X] - x) * (vals[X] - x); - RMS[Y] += (vals[Y] - y) * (vals[Y] - y); - RMS[Z] += (vals[Z] - z) * (vals[Z] - z); - } - for (i = 0; i < 3; i++) { - if (RMS[i] > accelRmsThresh * accelRmsThresh - * test_setup.accel_samples) { - MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " - "(threshold = %.2f)\n", - a_name[i], sqrt(RMS[i] / test_setup.accel_samples), - accelRmsThresh); - goto accel_error; - } - } - MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", - sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES), - sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES), - sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES)); - } - - return 0; /* success */ - -accel_error: /* error */ - bias[0] = bias[1] = bias[2] = 0; - return 1; -} - -/** - * @brief an user-space substitute of the power management function(s) - * in mldl_cfg.c for self test usage. - * Wake up and sleep the device, whether that is MPU3050, MPU6050A2, - * or MPU6050B1. - * @param mlsl_handle - * a file handle for the serial communication port used to - * communicate with the MPU device. - * @param power_level - * the power state to change the device into. Currently only 0 or - * 1 are supported, for sleep and full-power respectively. - * @return 0 on success; a non-zero error code on error. - */ -static inv_error_t inv_device_power_mgmt(void *mlsl_handle, - uint_fast8_t power_level) -{ - inv_error_t result; - static unsigned char pwr_mgm; - unsigned char b; - - if (power_level != 0 && power_level != 1) { - return INV_ERROR_INVALID_PARAMETER; - } - - if (power_level) { - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 1, &pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* reset */ - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET); -#ifndef CONFIG_MPU_SENSORS_MPU6050A2 - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#endif - inv_sleep(5); - - /* re-read power mgmt reg - - it may have reset after H_RESET is applied */ - result = inv_serial_read( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 1, &b); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - /* wake up */ -#ifdef CONFIG_MPU_SENSORS_MPU6050A2 - if ((b & BITS_PWRSEL) != BITS_PWRSEL) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, BITS_PWRSEL); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } -#else - if (pwr_mgm & BIT_SLEEP) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, 0x00); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } -#endif - inv_sleep(60); - -#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \ - defined(CONFIG_MPU_SENSORS_MPU6050B1) - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#endif - } else { - /* restore the power state the part was found in */ -#ifdef CONFIG_MPU_SENSORS_MPU6050A2 - if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } -#else - if (pwr_mgm & BIT_SLEEP) { - result = inv_serial_single_write( - mlsl_handle, mputestCfgPtr->addr, - MPUREG_PWR_MGM, pwr_mgm); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } -#endif - } - - return INV_SUCCESS; -} - -/** - * @brief The main entry point of the MPU Self Test, triggering the run of - * the single tests, for gyros and accelerometers. - * Prepares the MPU for the test, taking the device out of low power - * state if necessary, switching the MPU secondary I2C interface into - * bypass mode and restoring the original power state at the end of - * the test. - * This function is also responsible for encoding the output of each - * test in the correct format as it is stored on the file/medium of - * choice (according to inv_serial_write_cal() function). - * The format needs to stay perfectly consistent with the one expected - * by the corresponding loader in ml_stored_data.c; currectly the - * loaded in use is inv_load_cal_V1 (record type 1 - initial - * calibration). - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set threshouds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro() function. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * NOTE: for MPU6050 devices, this parameter is currently - * ignored. - * - * @return 0 on success. A non-zero error code on error. - * Propagates the errors from the tests up to the caller. - */ -int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result) -{ - int result = 0; - - short temp_avg; - short gyro_biases[3] = {0, 0, 0}; - short accel_biases[3] = {0, 0, 0}; - - unsigned long testStart = inv_get_tick_count(); - long accelSens[3] = {0}; - int ptr; - int tmp; - long long lltmp; - uint32_t chk; - - MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n", - DEF_TEST_TIME_PER_AXIS / 600); - MPL_LOGI("\n"); - - result = inv_device_power_mgmt(mlsl_handle, TRUE); - -#ifdef TRACK_IDS - result = test_get_mpu_id(mlsl_handle); - if (result != INV_SUCCESS) { - MPL_LOGI("Could not read the device's unique ID\n"); - MPL_LOGI("\n"); - return result; - } -#endif - - /* adjust the gyro sensitivity according to the gyro_sens_trim value */ - adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f; - test_setup.gyro_fs = (int)(32768.f / adjGyroSens); - - /* collect gyro and temperature data */ -#ifdef CONFIG_MPU_SENSORS_MPU3050 - result = inv_test_gyro_3050(mlsl_handle, - gyro_biases, &temp_avg, provide_result); -#else - MPL_LOGW_IF(provide_result, - "Self Test for MPU-6050 devices is for bias correction only: " - "no test PASS/FAIL result will be provided\n"); - result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg); -#endif - - MPL_LOGI("\n"); - MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart); - if (result) - return result; - - /* collect accel data. if this step is skipped, - ensure the array still contains zeros. */ - if (mputestCfgPtr->accel != NULL) { - float fs; - RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs); - accelSens[0] = (long)(32768L / fs); - accelSens[1] = (long)(32768L / fs); - accelSens[2] = (long)(32768L / fs); -#if defined CONFIG_MPU_SENSORS_MPU6050B1 - if (MPL_PROD_KEY(mputestCfgPtr->product_id, - mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) { - accelSens[2] /= 2; - } else { - unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim; - accelSens[0] /= trim_corr; - accelSens[1] /= trim_corr; - accelSens[2] /= trim_corr; - } -#endif - } else { - /* would be 0, but 1 to avoid divide-by-0 below */ - accelSens[0] = accelSens[1] = accelSens[2] = 1; - } -#ifdef CONFIG_MPU_SENSORS_MPU3050 - result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], - provide_result); -#else - result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], - FALSE); -#endif - if (result) - return result; - - result = inv_device_power_mgmt(mlsl_handle, FALSE); - if (result) - return result; - - ptr = 0; - dataStore[ptr++] = 0; /* total len of factory cal */ - dataStore[ptr++] = 0; - dataStore[ptr++] = 0; - dataStore[ptr++] = ML_INIT_CAL_LEN; - dataStore[ptr++] = 0; - dataStore[ptr++] = 5; /* record type 5 - initial calibration */ - - tmp = temp_avg; /* temperature */ - if (tmp < 0) tmp += 2 << 16; - USHORT_TO_CHARS(&dataStore[ptr], tmp); - ptr += 2; - - /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */ - lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - - lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */ - if (lltmp < 0) lltmp += 1LL << 32; - UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); - ptr += 4; - - /* add a checksum for data */ - chk = inv_checksum( - dataStore + INV_CAL_HDR_LEN, - ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN); - UINT_TO_CHARS(&dataStore[ptr], chk); - ptr += 4; - - if (ptr != ML_INIT_CAL_LEN) { - MPL_LOGI("Invalid calibration data length: exp %d, got %d\n", - ML_INIT_CAL_LEN, ptr); - return -1; - } - - return result; -} - -/** - * @brief The main test API. Runs the MPU Self Test and, if successful, - * stores the encoded initial calibration data on the final storage - * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE - * define in your mlsl implementation). - * - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param provide_result - * If 1: - * perform and analyze the offset, drive frequency, and noise - * calculation and compare it against set threshouds. Report - * also the final result using a bit-mask like error code as - * described in the inv_test_gyro() function. - * When 0: - * skip the noise and drive frequency calculation and pass/fail - * assessment. It simply calculates the gyro and accel biases. - * - * @return 0 on success or a non-zero error code from the callees on error. - */ -inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result) -{ - int result; - - result = inv_mpu_test(mlsl_handle, provide_result); - if (provide_result) { - MPL_LOGI("\n"); - if (result == 0) { - MPL_LOGI("Test : PASSED\n"); - } else { - MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result); - return result; /* abort writing the calibration if the - test is not successful */ - } - MPL_LOGI("\n"); - } else { - MPL_LOGI("\n"); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - - result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN); - if (result) { - MPL_LOGI("Error : cannot write calibration on file - error %d\n", - result); - return result; - } - - return INV_SUCCESS; -} - - - -/* ----------------------------------------------------------------------- - accel interface functions - -----------------------------------------------------------------------*/ - -/** - * @internal - * @brief Reads data for X, Y, and Z axis from the accelerometer device. - * Used only if an accelerometer has been setup using the - * inv_test_setup_accel() API. - * Takes care of the accelerometer endianess according to how the - * device has been described in the corresponding accelerometer driver - * file. - * @param mlsl_handle - * serial interface handle to allow serial communication with the - * device, both gyro and accelerometer. - * @param slave - * a pointer to the descriptor of the slave accelerometer device - * in use. Contains the necessary information to operate, read, - * and communicate with the accelerometer device of choice. - * See the declaration of struct ext_slave_descr in mpu.h. - * @param pdata - * a pointer to the platform info of the slave accelerometer - * device in use. Describes how the device is oriented and - * mounted on host platform's PCB. - * @param vals - * output pointer to return the accelerometer's X, Y, and Z axis - * sensor data collected. - * @return 0 on success or a non-zero error code on error. - */ -static inv_error_t test_get_data( - void *mlsl_handle, - struct mldl_cfg *mputestCfgPtr, - short *vals) -{ - inv_error_t result; - unsigned char data[20]; - struct ext_slave_descr *slave = mputestCfgPtr->accel; -#ifndef CONFIG_MPU_SENSORS_MPU3050 - struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel; -#endif - -#ifdef CONFIG_MPU_SENSORS_MPU3050 - result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, - 6, data); -#else - result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg, - slave->read_len, data); -#endif - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - if (VERBOSE_OUT) { - MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n", - ACCEL_UNPACK(data)); - } - - if (CHECK_NACKS(data)) { - MPL_LOGI("Error fetching data from the accelerometer : " - "all bytes read 0xff\n"); - return INV_ERROR_SERIAL_READ; - } - - if (slave->endian == EXT_SLAVE_BIG_ENDIAN) { - vals[0] = CHARS_TO_SHORT(&data[0]); - vals[1] = CHARS_TO_SHORT(&data[2]); - vals[2] = CHARS_TO_SHORT(&data[4]); - } else { - vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]); - vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]); - vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]); - } - - if (VERBOSE_OUT) { - MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", - vals[0], vals[1], vals[2]); - } - return INV_SUCCESS; -} - -#ifdef __cplusplus -} -#endif - -/** - * @} - */ - diff --git a/60xx/mlsdk/mlutils/mputest.h b/60xx/mlsdk/mlutils/mputest.h deleted file mode 100644 index d3347c5..0000000 --- a/60xx/mlsdk/mlutils/mputest.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * - * $Id: mputest.h 4051 2010-11-19 04:51:58Z mcaramello $ - * - *****************************************************************************/ - -#ifndef MPUTEST_H -#define MPUTEST_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mlsl.h" -#include "mldl_cfg.h" -#include "mputest_legacy.h" - -/* user facing APIs */ -inv_error_t inv_factory_calibrate(void *mlsl_handle, - uint_fast8_t provide_result); -void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, - int p_thresh, float total_time_tol, - int bias_thresh, float rms_thresh, - float sp_shift_thresh, - unsigned short accel_samples); - -/* additional functions */ -int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result); - - -#ifdef __cplusplus -} -#endif - -#endif /* MPUTEST_H */ - diff --git a/60xx/mlsdk/mlutils/slave.h b/60xx/mlsdk/mlutils/slave.h deleted file mode 100644 index 45449f6..0000000 --- a/60xx/mlsdk/mlutils/slave.h +++ /dev/null @@ -1,188 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: slave.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#ifndef SLAVE_H -#define SLAVE_H - -/** - * @addtogroup SLAVEDL - * - * @{ - * @file slave.h - * @brief Top level descriptions for Accelerometer support - * - */ - -#include "mltypes.h" -#include "mpu.h" - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -/*--- default accel support - selection ---*/ -#define ACCEL_ST_LIS331 0 -#define ACCEL_KIONIX_KXTF9 1 -#define ACCEL_BOSCH 0 -#define ACCEL_ADI 0 - -#define ACCEL_SLAVEADDR_INVALID 0x00 - -#define ACCEL_SLAVEADDR_LIS331 0x18 -#define ACCEL_SLAVEADDR_LSM303 0x18 -#define ACCEL_SLAVEADDR_LIS3DH 0x18 -#define ACCEL_SLAVEADDR_KXSD9 0x18 -#define ACCEL_SLAVEADDR_KXTF9 0x0F -#define ACCEL_SLAVEADDR_BMA150 0x38 -#define ACCEL_SLAVEADDR_BMA222 0x08 -#define ACCEL_SLAVEADDR_BMA250 0x18 -#define ACCEL_SLAVEADDR_ADXL34X 0x53 -#define ACCEL_SLAVEADDR_ADXL34X_ALT 0x1D /* alternative addr */ -#define ACCEL_SLAVEADDR_MMA8450 0x1C -#define ACCEL_SLAVEADDR_MMA845X 0x1C - -#define ACCEL_SLAVEADDR_INVENSENSE 0x68 -/* - Define default accelerometer to use if no selection is made -*/ -#if ACCEL_ST_LIS331 -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LIS331 -#define DEFAULT_ACCEL_ID ACCEL_ID_LIS331 -#endif - -#if ACCEL_ST_LSM303 -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LSM303 -#define DEFAULT_ACCEL_ID ACCEL_ID_LSM303A -#endif - -#if ACCEL_KIONIX_KXSD9 -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXSD9 -#define DEFAULT_ACCEL_ID ACCEL_ID_KXSD9 -#endif - -#if ACCEL_KIONIX_KXTF9 -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXTF9 -#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 -#endif - -#if ACCEL_BOSCH -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA150 -#define DEFAULT_ACCEL_ID ACCEL_ID_BMA150 -#endif - -#if ACCEL_BMA222 -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA222 -#define DEFAULT_ACCEL_ID ACCEL_ID_BMA222 -#endif - -#if ACCEL_BOSCH -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA250 -#define DEFAULT_ACCEL_ID ACCEL_ID_BMA250 -#endif - -#if ACCEL_ADI -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_ADXL34X -#define DEFAULT_ACCEL_ID ACCEL_ID_ADXL34X -#endif - -#if ACCEL_MMA8450 -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA8450 -#define DEFAULT_ACCEL_ID ACCEL_ID_MMA8450 -#endif - -#if ACCEL_MMA845X -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA845X -#define DEFAULT_ACCEL_ID ACCEL_ID_MMA845X -#endif - -/*--- if no default accelerometer was selected ---*/ -#ifndef DEFAULT_ACCEL_SLAVEADDR -#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_INVALID -#endif - -#define USE_COMPASS_AICHI 0 -#define USE_COMPASS_AKM 0 -#define USE_COMPASS_YAS529 0 -#define USE_COMPASS_YAS530 0 -#define USE_COMPASS_HMC5883 0 -#define USE_COMPASS_MMC314X 0 -#define USE_COMPASS_HSCDTD002B 0 -#define USE_COMPASS_HSCDTD004A 0 - -#define COMPASS_SLAVEADDR_INVALID 0x00 -#define COMPASS_SLAVEADDR_AKM_BASE 0x0C -#define COMPASS_SLAVEADDR_AKM 0x0E -#define COMPASS_SLAVEADDR_AMI304 0x0E -#define COMPASS_SLAVEADDR_AMI305 0x0F /*Slave address for AMI 305/306*/ -#define COMPASS_SLAVEADDR_AMI306 0x0E /*Slave address for AMI 305/306*/ -#define COMPASS_SLAVEADDR_YAS529 0x2E -#define COMPASS_SLAVEADDR_YAS530 0x2E -#define COMPASS_SLAVEADDR_HMC5883 0x1E -#define COMPASS_SLAVEADDR_MMC314X 0x30 -#define COMPASS_SLAVEADDR_HSCDTD00XX 0x0C - -/* - Define default compass to use if no selection is made -*/ - #if USE_COMPASS_AKM - #define DEFAULT_COMPASS_TYPE COMPASS_ID_AK8975 - #endif - - #if USE_COMPASS_AICHI - #define DEFAULT_COMPASS_TYPE COMPASS_ID_AMI30X - #endif - - #if USE_COMPASS_YAS529 - #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS529 - #endif - - #if USE_COMPASS_YAS530 - #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS530 - #endif - - #if USE_COMPASS_HMC5883 - #define DEFAULT_COMPASS_TYPE COMPASS_ID_HMC5883 - #endif - -#if USE_COMPASS_MMC314X -#define DEFAULT_COMPASS_TYPE COMPASS_ID_MMC314X -#endif - -#if USE_COMPASS_HSCDTD002B -#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD002B -#endif - -#if USE_COMPASS_HSCDTD004A -#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD004A -#endif - -#ifndef DEFAULT_COMPASS_TYPE -#define DEFAULT_COMPASS_TYPE ID_INVALID -#endif - - -#endif // SLAVE_H - -/** - * @} - */ diff --git a/60xx/mlsdk/platform/include/i2c.h b/60xx/mlsdk/platform/include/i2c.h deleted file mode 100644 index 39dd255..0000000 --- a/60xx/mlsdk/platform/include/i2c.h +++ /dev/null @@ -1,133 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: i2c.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#ifndef _I2C_H -#define _I2C_H - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* ------------ */ -/* - Defines. - */ -/* ------------ */ - -/* - Error Codes. - */ - -#define I2C_SUCCESS 0 -#define I2C_ERROR 1 - -/* ---------- */ -/* - Enums. - */ -/* ---------- */ - -/* --------------- */ -/* - Structures. - */ -/* --------------- */ - -#define I2C_RBUFF_MAX 128 -#define I2C_RBUFF_SIZE 17 - -#ifdef BB_I2C -#define I2C_RBUFF_DYNAMIC 0 -#else -#define I2C_RBUFF_DYNAMIC 1 -#endif - -typedef struct { - - HANDLE i2cHndl; - HANDLE hDevice; // handle to the drive to be examined - - MLU8 readBuffer[1024]; - MLU8 writeBuffer[1024]; - - MLU16 rBuffRIndex; - MLU16 rBuffWIndex; -#if !I2C_RBUFF_DYNAMIC - MLU8 rBuff[I2C_RBUFF_MAX][I2C_RBUFF_SIZE]; -#else - MLU8 *rBuff; -#endif - MLU16 rBuffMax; - MLU16 rBuffNumBytes; - - MLU8 runThread; - MLU8 autoProcess; - -} I2C_Vars_t; - -/* --------------------- */ -/* - Function p-types. - */ -/* --------------------- */ - -#if (defined(BB_I2C)) -void set_i2c_open_bind_cb(int (*func)(unsigned int i2c_slave_addr)); -void set_i2c_open_cb(int (*func)(const char *dev, int rw)); -void set_i2c_close_cb(int (*func)(int fd)); -void set_i2c_lltransfer_cb(int (*func)(int fd, int client_addr, const char *write_buf, unsigned int write_len, - char *read_buf, unsigned int read_len )); -void set_i2c_write_register_cb(int (*func)(int fd, int client_addr, unsigned char reg, unsigned char value)); -void set_i2c_read_register_cb(unsigned char (*func)(int fd, int client_addr, unsigned char reg)); -void set_i2c_dump_register_cb(int (*func)(int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len)); - -int _i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value); -unsigned char _i2c_read_register (int fd, int client_addr, unsigned char reg); -int i2c_lltransfer (int fd, int client_addr, const char *write_buf, unsigned int write_len, - char *read_buf, unsigned int read_len ); -int i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value); -unsigned char i2c_read_register (int fd, int client_addr, unsigned char reg); -int i2c_dump_register (int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len); -int i2c_open (const char *dev, int rw); -int i2c_close (int fd); -int i2c_open_bind (unsigned int i2c_slave_addr); -#endif - -int I2COpen (unsigned char autoProcess, unsigned char createThread); -int I2CClose (void); -int I2CDeviceIoControl(void); -int I2CRead (void); -int I2CWrite (void); -int I2CSetBufferSize (unsigned short bufferSize); -int I2CBufferUpdate (void); -int I2CHandler (void); -int I2CReadBuffer (unsigned short cnt, unsigned char bufferMode, unsigned char *rBuff); -int I2CEmptyBuffer (void); -int I2CPktsInBuffer (unsigned short *pktsInBuffer); -int I2CCreateMutex (void); -int I2CLockMutex (void); -int I2CUnlockMutex (void); - -int I2CWriteBurst (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data); -int I2CReadBurst (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data); - -int I2COpenBB (void); -int I2CCloseBB (int i2cHandle); - -#ifdef __cplusplus -} -#endif - -#endif /* _TEMPLATE_H */ diff --git a/60xx/mlsdk/platform/include/linux/mpu.h b/60xx/mlsdk/platform/include/linux/mpu.h deleted file mode 100644 index 04fa7b6..0000000 --- a/60xx/mlsdk/platform/include/linux/mpu.h +++ /dev/null @@ -1,335 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef __MPU_H_ -#define __MPU_H_ - -#ifdef __KERNEL__ -#include <linux/types.h> -#include <linux/ioctl.h> -#elif defined LINUX -#include <sys/ioctl.h> -#endif - -/* Number of axes on each sensor */ -#define GYRO_NUM_AXES (3) -#define ACCEL_NUM_AXES (3) -#define COMPASS_NUM_AXES (3) - -struct mpu_read_write { - /* Memory address or register address depending on ioctl */ - unsigned short address; - unsigned short length; - unsigned char *data; -}; - -enum mpuirq_data_type { - MPUIRQ_DATA_TYPE_MPU_IRQ, - MPUIRQ_DATA_TYPE_SLAVE_IRQ, - MPUIRQ_DATA_TYPE_PM_EVENT, - MPUIRQ_DATA_TYPE_NUM_TYPES, -}; - -/* User space PM event notification */ -#define MPU_PM_EVENT_SUSPEND_PREPARE (3) -#define MPU_PM_EVENT_POST_SUSPEND (4) - -struct mpuirq_data { - int interruptcount; - unsigned long long irqtime; - int data_type; - long data; -}; - -enum ext_slave_config_key { - MPU_SLAVE_CONFIG_ODR_SUSPEND, - MPU_SLAVE_CONFIG_ODR_RESUME, - MPU_SLAVE_CONFIG_FSR_SUSPEND, - MPU_SLAVE_CONFIG_FSR_RESUME, - MPU_SLAVE_CONFIG_MOT_THS, - MPU_SLAVE_CONFIG_NMOT_THS, - MPU_SLAVE_CONFIG_MOT_DUR, - MPU_SLAVE_CONFIG_NMOT_DUR, - MPU_SLAVE_CONFIG_IRQ_SUSPEND, - MPU_SLAVE_CONFIG_IRQ_RESUME, - MPU_SLAVE_WRITE_REGISTERS, - MPU_SLAVE_READ_REGISTERS, - /* AMI 306 specific config keys */ - MPU_SLAVE_PARAM, - MPU_SLAVE_WINDOW, - MPU_SLAVE_READWINPARAMS, - MPU_SLAVE_SEARCHOFFSET, - /* AKM specific config keys */ - MPU_SLAVE_READ_SCALE, - /* YAS specific config keys */ - MPU_SLAVE_OFFSET_VALS, - MPU_SLAVE_RANGE_CHECK, - - MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS, -}; - -/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */ -enum ext_slave_config_irq_type { - MPU_SLAVE_IRQ_TYPE_NONE, - MPU_SLAVE_IRQ_TYPE_MOTION, - MPU_SLAVE_IRQ_TYPE_DATA_READY, -}; - -/* Structure for the following IOCTS's - * MPU_CONFIG_ACCEL - * MPU_CONFIG_COMPASS - * MPU_CONFIG_PRESSURE - * MPU_GET_CONFIG_ACCEL - * MPU_GET_CONFIG_COMPASS - * MPU_GET_CONFIG_PRESSURE - * - * @key one of enum ext_slave_config_key - * @len length of data pointed to by data - * @apply zero if communication with the chip is not necessary, false otherwise - * This flag can be used to select cached data or to refresh cashed data - * cache data to be pushed later or push immediately. If true and the - * slave is on the secondary bus the MPU will first enger bypass mode - * before calling the slaves .config or .get_config funcion - * @data pointer to the data to confgure or get - */ -struct ext_slave_config { - int key; - int len; - int apply; - void *data; -}; - -enum ext_slave_type { - EXT_SLAVE_TYPE_GYROSCOPE, - EXT_SLAVE_TYPE_ACCELEROMETER, - EXT_SLAVE_TYPE_COMPASS, - EXT_SLAVE_TYPE_PRESSURE, - /*EXT_SLAVE_TYPE_TEMPERATURE */ - - EXT_SLAVE_NUM_TYPES -}; - -enum ext_slave_id { - ID_INVALID = 0, - - ACCEL_ID_LIS331, - ACCEL_ID_LSM303A, - ACCEL_ID_LIS3DH, - ACCEL_ID_KXSD9, - ACCEL_ID_KXTF9, - ACCEL_ID_BMA150, - ACCEL_ID_BMA222, - ACCEL_ID_BMA250, - ACCEL_ID_ADXL34X, - ACCEL_ID_MMA8450, - ACCEL_ID_MMA845X, - ACCEL_ID_MPU6050, - - COMPASS_ID_AK8975, - COMPASS_ID_AMI30X, - COMPASS_ID_AMI306, - COMPASS_ID_YAS529, - COMPASS_ID_YAS530, - COMPASS_ID_HMC5883, - COMPASS_ID_LSM303M, - COMPASS_ID_MMC314X, - COMPASS_ID_HSCDTD002B, - COMPASS_ID_HSCDTD004A, - - PRESSURE_ID_BMA085, -}; - -enum ext_slave_endian { - EXT_SLAVE_BIG_ENDIAN, - EXT_SLAVE_LITTLE_ENDIAN, - EXT_SLAVE_FS8_BIG_ENDIAN, - EXT_SLAVE_FS16_BIG_ENDIAN, -}; - -enum ext_slave_bus { - EXT_SLAVE_BUS_INVALID = -1, - EXT_SLAVE_BUS_PRIMARY = 0, - EXT_SLAVE_BUS_SECONDARY = 1 -}; - - -/** - * struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050 - * slave devices - * - * @get_slave_descr: Function pointer to retrieve the struct ext_slave_descr - * for this slave - * @irq: the irq number attached to the slave if any. - * @adapt_num: the I2C adapter number. - * @bus: the bus the slave is attached to: enum ext_slave_bus - * @address: the I2C slave address of the slave device. - * @orientation: the mounting matrix of the device relative to MPU. - * @irq_data: private data for the slave irq handler - * @private_data: additional data, user customizable. Not touched by the MPU - * driver. - * - * The orientation matricies are 3x3 rotation matricies - * that are applied to the data to rotate from the mounting orientation to the - * platform orientation. The values must be one of 0, 1, or -1 and each row and - * column should have exactly 1 non-zero value. - */ -struct ext_slave_platform_data { - struct ext_slave_descr *(*get_slave_descr) (void); - int irq; - int adapt_num; - int bus; - unsigned char address; - signed char orientation[9]; - void *irq_data; - void *private_data; -}; - -struct fix_pnt_range { - long mantissa; - long fraction; -}; - -static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng) -{ - return (long)(rng.mantissa * 1000 + rng.fraction / 10); -} - -struct ext_slave_read_trigger { - unsigned char reg; - unsigned char value; -}; - -/** - * struct ext_slave_descr - Description of the slave device for programming. - * - * @suspend: function pointer to put the device in suspended state - * @resume: function pointer to put the device in running state - * @read: function that reads the device data - * @init: function used to preallocate memory used by the driver - * @exit: function used to free memory allocated for the driver - * @config: function used to configure the device - * @get_config:function used to get the device's configuration - * - * @name: text name of the device - * @type: device type. enum ext_slave_type - * @id: enum ext_slave_id - * @reg: starting register address to retrieve data. - * @len: length in bytes of the sensor data. Should be 6. - * @endian: byte order of the data. enum ext_slave_endian - * @range: full scale range of the slave ouput: struct fix_pnt_range - * @trigger: If reading data first requires writing a register this is the - * data to write. - * - * Defines the functions and information about the slave the mpu3050 and - * mpu6050 needs to use the slave device. - */ -struct ext_slave_descr { - int (*init) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*exit) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*suspend) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*resume) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata); - int (*read) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - unsigned char *data); - int (*config) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - struct ext_slave_config *config); - int (*get_config) (void *mlsl_handle, - struct ext_slave_descr *slave, - struct ext_slave_platform_data *pdata, - struct ext_slave_config *config); - - char *name; - unsigned char type; - unsigned char id; - unsigned char read_reg; - unsigned int read_len; - unsigned char endian; - struct fix_pnt_range range; - struct ext_slave_read_trigger *trigger; -}; - -/** - * struct mpu_platform_data - Platform data for the mpu driver - * @int_config: Bits [7:3] of the int config register. - * @orientation: Orientation matrix of the gyroscope - * @level_shifter: 0: VLogic, 1: VDD - * @accel: Accel platform data - * @compass: Compass platform data - * @pressure: Pressure platform data - * - * Contains platform specific information on how to configure the MPU3050 to - * work on this platform. The orientation matricies are 3x3 rotation matricies - * that are applied to the data to rotate from the mounting orientation to the - * platform orientation. The values must be one of 0, 1, or -1 and each row and - * column should have exactly 1 non-zero value. - */ -struct mpu_platform_data { - unsigned char int_config; - signed char orientation[GYRO_NUM_AXES * GYRO_NUM_AXES]; - unsigned char level_shifter; - struct ext_slave_platform_data accel; - struct ext_slave_platform_data compass; - struct ext_slave_platform_data pressure; -}; - -#if defined __KERNEL__ || defined LINUX -#define MPU_IOCTL (0x81) /* Magic number for MPU Iocts */ -/* IOCTL commands for /dev/mpu */ -#define MPU_SET_MPU_CONFIG _IOWR(MPU_IOCTL, 0x00, struct mldl_cfg) -#define MPU_GET_MPU_CONFIG _IOW(MPU_IOCTL, 0x00, struct mldl_cfg) - -#define MPU_SET_PLATFORM_DATA _IOWR(MPU_IOCTL, 0x01, struct mldl_cfg) - -#define MPU_READ _IOWR(MPU_IOCTL, 0x10, struct mpu_read_write) -#define MPU_WRITE _IOW(MPU_IOCTL, 0x10, struct mpu_read_write) -#define MPU_READ_MEM _IOWR(MPU_IOCTL, 0x11, struct mpu_read_write) -#define MPU_WRITE_MEM _IOW(MPU_IOCTL, 0x11, struct mpu_read_write) -#define MPU_READ_FIFO _IOWR(MPU_IOCTL, 0x12, struct mpu_read_write) -#define MPU_WRITE_FIFO _IOW(MPU_IOCTL, 0x12, struct mpu_read_write) - -#define MPU_READ_COMPASS _IOR(MPU_IOCTL, 0x12, unsigned char) -#define MPU_READ_ACCEL _IOR(MPU_IOCTL, 0x13, unsigned char) -#define MPU_READ_PRESSURE _IOR(MPU_IOCTL, 0x14, unsigned char) - -#define MPU_CONFIG_ACCEL _IOW(MPU_IOCTL, 0x20, struct ext_slave_config) -#define MPU_CONFIG_COMPASS _IOW(MPU_IOCTL, 0x21, struct ext_slave_config) -#define MPU_CONFIG_PRESSURE _IOW(MPU_IOCTL, 0x22, struct ext_slave_config) - -#define MPU_GET_CONFIG_ACCEL _IOWR(MPU_IOCTL, 0x20, struct ext_slave_config) -#define MPU_GET_CONFIG_COMPASS _IOWR(MPU_IOCTL, 0x21, struct ext_slave_config) -#define MPU_GET_CONFIG_PRESSURE _IOWR(MPU_IOCTL, 0x22, struct ext_slave_config) - -#define MPU_SUSPEND _IO(MPU_IOCTL, 0x30) -#define MPU_RESUME _IO(MPU_IOCTL, 0x31) -/* Userspace PM Event response */ -#define MPU_PM_EVENT_HANDLED _IO(MPU_IOCTL, 0x32) - -#endif - -#endif /* __MPU_H_ */ diff --git a/60xx/mlsdk/platform/include/log.h b/60xx/mlsdk/platform/include/log.h deleted file mode 100644 index 8485074..0000000 --- a/60xx/mlsdk/platform/include/log.h +++ /dev/null @@ -1,344 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/* - * This file incorporates work covered by the following copyright and - * permission notice: - * - * Copyright (C) 2005 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* - * C/C++ logging functions. See the logging documentation for API details. - * - * We'd like these to be available from C code (in case we import some from - * somewhere), so this has a C interface. - * - * The output will be correct when the log file is shared between multiple - * threads and/or multiple processes so long as the operating system - * supports O_APPEND. These calls have mutex-protected data structures - * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. - */ -#ifndef _LIBS_CUTILS_MPL_LOG_H -#define _LIBS_CUTILS_MPL_LOG_H - -#include "mltypes.h" -#include <stdarg.h> - -#ifdef ANDROID -#ifdef NDK_BUILD -#include "log_macros.h" -#else -#include <utils/Log.h> /* For the LOG macro */ -#endif -#endif - -#ifdef __KERNEL__ -#include <linux/kernel.h> -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* --------------------------------------------------------------------- */ - -/* - * Normally we strip MPL_LOGV (VERBOSE messages) from release builds. - * You can modify this (for example with "#define MPL_LOG_NDEBUG 0" - * at the top of your source file) to change that behavior. - */ -#ifndef MPL_LOG_NDEBUG -#ifdef NDEBUG -#define MPL_LOG_NDEBUG 1 -#else -#define MPL_LOG_NDEBUG 0 -#endif -#endif - -#ifdef __KERNEL__ -#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE -#define MPL_LOG_DEFAULT KERN_DEFAULT -#define MPL_LOG_VERBOSE KERN_CONT -#define MPL_LOG_DEBUG KERN_NOTICE -#define MPL_LOG_INFO KERN_INFO -#define MPL_LOG_WARN KERN_WARNING -#define MPL_LOG_ERROR KERN_ERR -#define MPL_LOG_SILENT MPL_LOG_VERBOSE - -#else - /* Based off the log priorities in android - /system/core/include/android/log.h */ -#define MPL_LOG_UNKNOWN (0) -#define MPL_LOG_DEFAULT (1) -#define MPL_LOG_VERBOSE (2) -#define MPL_LOG_DEBUG (3) -#define MPL_LOG_INFO (4) -#define MPL_LOG_WARN (5) -#define MPL_LOG_ERROR (6) -#define MPL_LOG_SILENT (8) -#endif - - -/* - * This is the local tag used for the following simplified - * logging macros. You can change this preprocessor definition - * before using the other macros to change the tag. - */ -#ifndef MPL_LOG_TAG -#ifdef __KERNEL__ -#define MPL_LOG_TAG -#else -#define MPL_LOG_TAG NULL -#endif -#endif - -/* --------------------------------------------------------------------- */ - -/* - * Simplified macro to send a verbose log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGV -#if MPL_LOG_NDEBUG -#define MPL_LOGV(fmt, ...) \ - do { \ - if (0) \ - MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ - } while (0) -#else -#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef CONDITION -#define CONDITION(cond) ((cond) != 0) -#endif - -#ifndef MPL_LOGV_IF -#if MPL_LOG_NDEBUG -#define MPL_LOGV_IF(cond, fmt, ...) \ - do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0) -#else -#define MPL_LOGV_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif -#endif - -/* - * Simplified macro to send a debug log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGD -#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif - -#ifndef MPL_LOGD_IF -#define MPL_LOGD_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* - * Simplified macro to send an info log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGI -#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif - -#ifndef MPL_LOGI_IF -#define MPL_LOGI_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* - * Simplified macro to send a warning log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGW -#ifdef __KERNEL__ -#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__) -#else -#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef MPL_LOGW_IF -#define MPL_LOGW_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* - * Simplified macro to send an error log message using the current MPL_LOG_TAG. - */ -#ifndef MPL_LOGE -#ifdef __KERNEL__ -#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__) -#else -#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) -#endif -#endif - -#ifndef MPL_LOGE_IF -#define MPL_LOGE_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ - : (void)0) -#endif - -/* --------------------------------------------------------------------- */ - -/* - * Log a fatal error. If the given condition fails, this stops program - * execution like a normal assertion, but also generating the given message. - * It is NOT stripped from release builds. Note that the condition test - * is -inverted- from the normal assert() semantics. - */ -#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \ - ((CONDITION(cond)) \ - ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \ - fmt, ##__VA_ARGS__)) \ - : (void)0) - -#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \ - (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__))) - -/* - * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that - * are stripped out of release builds. - */ -#if MPL_LOG_NDEBUG -#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ - do { \ - if (0) \ - MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \ - } while (0) -#define MPL_LOG_FATAL(fmt, ...) \ - do { \ - if (0) \ - MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \ - } while (0) -#else -#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ - MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__) -#define MPL_LOG_FATAL(fmt, ...) \ - MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) -#endif - -/* - * Assertion that generates a log message when the assertion fails. - * Stripped out of release builds. Uses the current MPL_LOG_TAG. - */ -#define MPL_LOG_ASSERT(cond, fmt, ...) \ - MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__) - -/* --------------------------------------------------------------------- */ - -/* - * Basic log message macro. - * - * Example: - * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno); - * - * The second argument may be NULL or "" to indicate the "global" tag. - */ -#ifndef MPL_LOG -#define MPL_LOG(priority, tag, fmt, ...) \ - MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__) -#endif - -/* - * Log macro that allows you to specify a number for the priority. - */ -#ifndef MPL_LOG_PRI -#ifdef ANDROID -#define MPL_LOG_PRI(priority, tag, fmt, ...) \ - ALOG(priority, tag, fmt, ##__VA_ARGS__) -#elif defined __KERNEL__ -#define MPL_LOG_PRI(priority, tag, fmt, ...) \ - pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__) -#else -#define MPL_LOG_PRI(priority, tag, fmt, ...) \ - _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__) -#endif -#endif - -/* - * Log macro that allows you to pass in a varargs ("args" is a va_list). - */ -#ifndef MPL_LOG_PRI_VA -#ifdef ANDROID -#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ - android_vprintLog(priority, NULL, tag, fmt, args) -#elif defined __KERNEL__ -/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */ -#else -#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ - _MLPrintVaLog(priority, NULL, tag, fmt, args) -#endif -#endif - -/* --------------------------------------------------------------------- */ - -/* - * =========================================================================== - * - * The stuff in the rest of this file should not be used directly. - */ - -#ifndef ANDROID -int _MLPrintLog(int priority, const char *tag, const char *fmt, ...); -int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args); -/* Final implementation of actual writing to a character device */ -int _MLWriteLog(const char *buf, int buflen); -#endif - -static inline void __print_result_location(int result, - const char *file, - const char *func, int line) -{ - MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result); -} - -#define LOG_RESULT_LOCATION(condition) \ - do { \ - __print_result_location((int)(condition), __FILE__, \ - __func__, __LINE__); \ - } while (0) - - -#ifdef __cplusplus -} -#endif -#endif /* _LIBS_CUTILS_MPL_LOG_H */ diff --git a/60xx/mlsdk/platform/include/mlmath.h b/60xx/mlsdk/platform/include/mlmath.h deleted file mode 100644 index bf54870..0000000 --- a/60xx/mlsdk/platform/include/mlmath.h +++ /dev/null @@ -1,107 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#ifndef _ML_MATH_H_ -#define _ML_MATH_H_ - -#ifndef MLMATH -// This define makes Microsoft pickup things like M_PI -#define _USE_MATH_DEFINES -#include <math.h> - -#ifdef WIN32 -// Microsoft doesn't follow standards -#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5)))) -#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) -#endif - -#else // MLMATH - -#ifdef __cplusplus -extern "C" { -#endif -/* MPL needs below functions */ -double ml_asin(double); -double ml_atan(double); -double ml_atan2(double, double); -double ml_log(double); -double ml_sqrt(double); -double ml_ceil(double); -double ml_floor(double); -double ml_cos(double); -double ml_sin(double); -double ml_acos(double); -#ifdef __cplusplus -} // extern "C" -#endif - -/* - * We rename functions here to provide the hook for other - * customized math functions. - */ -#define sqrt(x) ml_sqrt(x) -#define log(x) ml_log(x) -#define asin(x) ml_asin(x) -#define atan(x) ml_atan(x) -#define atan2(x,y) ml_atan2(x,y) -#define ceil(x) ml_ceil(x) -#define floor(x) ml_floor(x) -#define fabs(x) (((x)<0)?-(x):(x)) -#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5)))) -#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) -#define cos(x) ml_cos(x) -#define sin(x) ml_sin(x) -#define acos(x) ml_acos(x) - -#define pow(x,y) ml_pow(x,y) - -#ifdef LINUX -/* stubs for float version of math functions */ -#define cosf(x) ml_cos(x) -#define sinf(x) ml_sin(x) -#define atan2f(x,y) ml_atan2(x,y) -#define sqrtf(x) ml_sqrt(x) -#endif - - - -#endif // MLMATH - -#ifndef M_PI -#define M_PI 3.14159265358979 -#endif - -#ifndef ABS -#define ABS(x) (((x)>=0)?(x):-(x)) -#endif - -#ifndef MIN -#define MIN(x,y) (((x)<(y))?(x):(y)) -#endif - -#ifndef MAX -#define MAX(x,y) (((x)>(y))?(x):(y)) -#endif - -/*---------------------------*/ -#endif /* !_ML_MATH_H_ */ diff --git a/60xx/mlsdk/platform/include/mlos.h b/60xx/mlsdk/platform/include/mlos.h deleted file mode 100644 index 0aeda96..0000000 --- a/60xx/mlsdk/platform/include/mlos.h +++ /dev/null @@ -1,109 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef _MLOS_H -#define _MLOS_H - -#ifndef __KERNEL__ -#include <stdio.h> -#endif - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(LINUX) || defined(__KERNEL__) -#include <stdint.h> -typedef uintptr_t HANDLE; -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /* - MLOSCreateFile defines. - */ - -#define MLOS_GENERIC_READ ((unsigned int)0x80000000) -#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) -#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) -#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) -#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) - - /* ---------- */ - /* - Enums. - */ - /* ---------- */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - -#ifndef __KERNEL__ -#include <string.h> - void *inv_malloc(unsigned int numBytes); - inv_error_t inv_free(void *ptr); - inv_error_t inv_create_mutex(HANDLE *mutex); - inv_error_t inv_lock_mutex(HANDLE mutex); - inv_error_t inv_unlock_mutex(HANDLE mutex); - FILE *inv_fopen(char *filename); - void inv_fclose(FILE *fp); - - inv_error_t inv_destroy_mutex(HANDLE handle); - - void inv_sleep(int mSecs); - unsigned long inv_get_tick_count(void); - - /* Kernel implmentations */ -#define GFP_KERNEL (0x70) - static inline void *kmalloc(size_t size, - unsigned int gfp_flags) - { - return inv_malloc((unsigned int)size); - } - static inline void *kzalloc(size_t size, unsigned int gfp_flags) - { - void *tmp = inv_malloc((unsigned int)size); - if (tmp) - memset(tmp, 0, size); - return tmp; - } - static inline void kfree(void *ptr) - { - inv_free(ptr); - } - static inline void msleep(long msecs) - { - inv_sleep(msecs); - } - static inline void udelay(unsigned long usecs) - { - inv_sleep((usecs + 999) / 1000); - } -#else -#include <linux/delay.h> -#endif - -#ifdef __cplusplus -} -#endif -#endif /* _MLOS_H */ diff --git a/60xx/mlsdk/platform/include/mlsl.h b/60xx/mlsdk/platform/include/mlsl.h deleted file mode 100644 index 535d117..0000000 --- a/60xx/mlsdk/platform/include/mlsl.h +++ /dev/null @@ -1,290 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef __MLSL_H__ -#define __MLSL_H__ - -/** - * @defgroup MLSL - * @brief Motion Library - Serial Layer. - * The Motion Library System Layer provides the Motion Library - * with the communication interface to the hardware. - * - * The communication interface is assumed to support serial - * transfers in burst of variable length up to - * SERIAL_MAX_TRANSFER_SIZE. - * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes. - * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will - * be subdivided in smaller transfers of length <= - * SERIAL_MAX_TRANSFER_SIZE. - * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to - * overcome any host processor transfer size limitation down to - * 1 B, the minimum. - * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor - * performance and efficiency while requiring higher resource usage - * (mostly buffering). A smaller value will increase overhead and - * decrease efficiency but allows to operate with more resource - * constrained processor and master serial controllers. - * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the - * mlsl.h header file and master serial controllers. - * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the - * mlsl.h header file. - * - * @{ - * @file mlsl.h - * @brief The Motion Library System Layer. - * - */ - -#include "mltypes.h" -#include <linux/mpu.h> - -#ifdef __cplusplus -extern "C" { -#endif - -/* - * NOTE : to properly support Yamaha compass reads, - * the max transfer size should be at least 9 B. - * Length in bytes, typically a power of 2 >= 2 - */ -#define SERIAL_MAX_TRANSFER_SIZE 128 - -#ifndef __KERNEL__ -/** - * inv_serial_open() - used to open the serial port. - * @port The COM port specification associated with the device in use. - * @sl_handle a pointer to the file handle to the serial device to be open - * for the communication. - * This port is used to send and receive data to the device. - * - * This function is called by inv_serial_start(). - * Unlike previous MPL Software releases, explicitly calling - * inv_serial_start() is mandatory to instantiate the communication - * with the device. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_open(char const *port, void **sl_handle); - -/** - * inv_serial_close() - used to close the serial port. - * @sl_handle a file handle to the serial device used for the communication. - * - * This port is used to send and receive data to the device. - * - * This function is called by inv_serial_stop(). - * Unlike previous MPL Software releases, explicitly calling - * inv_serial_stop() is mandatory to properly shut-down the - * communication with the device. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_close(void *sl_handle); - -/** - * inv_serial_reset() - used to reset any buffering the driver may be doing - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_reset(void *sl_handle); -#endif - -/** - * inv_serial_single_write() - used to write a single byte of data. - * @sl_handle pointer to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @register_addr Register address to write. - * @data Single byte of data to write. - * - * It is called by the MPL to write a single byte of data to the MPU. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_single_write( - void *sl_handle, - unsigned char slave_addr, - unsigned char register_addr, - unsigned char data); - -/** - * inv_serial_write() - used to write multiple bytes of data to registers. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @register_addr Register address to write. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_write( - void *sl_handle, - unsigned char slave_addr, - unsigned short length, - unsigned char const *data); - -/** - * inv_serial_read() - used to read multiple bytes of data from registers. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @register_addr Register address to read. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -inv_error_t inv_serial_read( - void *sl_handle, - unsigned char slave_addr, - unsigned char register_addr, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_read_mem() - used to read multiple bytes of data from the memory. - * This should be sent by I2C or SPI. - * - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @mem_addr The location in the memory to read from. - * @length Length of burst data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -inv_error_t inv_serial_read_mem( - void *sl_handle, - unsigned char slave_addr, - unsigned short mem_addr, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_write_mem() - used to write multiple bytes of data to the memory. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @mem_addr The location in the memory to write to. - * @length Length of burst data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -inv_error_t inv_serial_write_mem( - void *sl_handle, - unsigned char slave_addr, - unsigned short mem_addr, - unsigned short length, - unsigned char const *data); - -/** - * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -inv_error_t inv_serial_read_fifo( - void *sl_handle, - unsigned char slave_addr, - unsigned short length, - unsigned char *data); - -/** - * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo. - * @sl_handle a file handle to the serial device used for the communication. - * @slave_addr I2C slave address of device. - * @length Length of burst of data. - * @data Pointer to block of data. - * - * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. - */ -inv_error_t inv_serial_write_fifo( - void *sl_handle, - unsigned char slave_addr, - unsigned short length, - unsigned char const *data); - -#ifndef __KERNEL__ -/** - * inv_serial_read_cfg() - used to get the configuration data. - * @cfg Pointer to the configuration data. - * @len Length of the configuration data. - * - * Is called by the MPL to get the configuration data - * used by the motion library. - * This data would typically be saved in non-volatile memory. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len); - -/** - * inv_serial_write_cfg() - used to save the configuration data. - * @cfg Pointer to the configuration data. - * @len Length of the configuration data. - * - * Is called by the MPL to save the configuration data used by the - * motion library. - * This data would typically be saved in non-volatile memory. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len); - -/** - * inv_serial_read_cal() - used to get the calibration data. - * @cfg Pointer to the calibration data. - * @len Length of the calibration data. - * - * It is called by the MPL to get the calibration data used by the - * motion library. - * This data is typically be saved in non-volatile memory. - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len); - -/** - * inv_serial_write_cal() - used to save the calibration data. - * - * @cfg Pointer to the calibration data. - * @len Length of the calibration data. - * - * It is called by the MPL to save the calibration data used by the - * motion library. - * This data is typically be saved in non-volatile memory. - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len); - -/** - * inv_serial_get_cal_length() - Get the calibration length from the storage. - * @len lenght to be returned - * - * returns INV_SUCCESS if successful, a non-zero error code otherwise. - */ -inv_error_t inv_serial_get_cal_length(unsigned int *len); -#endif -#ifdef __cplusplus -} -#endif -/** - * @} - */ -#endif /* __MLSL_H__ */ diff --git a/60xx/mlsdk/platform/include/mltypes.h b/60xx/mlsdk/platform/include/mltypes.h deleted file mode 100644 index 90a126b..0000000 --- a/60xx/mlsdk/platform/include/mltypes.h +++ /dev/null @@ -1,265 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/** - * @defgroup MLERROR - * @brief Motion Library - Error definitions. - * Definition of the error codes used within the MPL and - * returned to the user. - * Every function tries to return a meaningful error code basing - * on the occuring error condition. The error code is numeric. - * - * The available error codes and their associated values are: - * - (0) INV_SUCCESS - * - (1) INV_ERROR - * - (2) INV_ERROR_INVALID_PARAMETER - * - (3) INV_ERROR_FEATURE_NOT_ENABLED - * - (4) INV_ERROR_FEATURE_NOT_IMPLEMENTED - * - (6) INV_ERROR_DMP_NOT_STARTED - * - (7) INV_ERROR_DMP_STARTED - * - (8) INV_ERROR_NOT_OPENED - * - (9) INV_ERROR_OPENED - * - (10) INV_ERROR_INVALID_MODULE - * - (11) INV_ERROR_MEMORY_EXAUSTED - * - (12) INV_ERROR_DIVIDE_BY_ZERO - * - (13) INV_ERROR_ASSERTION_FAILURE - * - (14) INV_ERROR_FILE_OPEN - * - (15) INV_ERROR_FILE_READ - * - (16) INV_ERROR_FILE_WRITE - * - (17) INV_ERROR_INVALID_CONFIGURATION - * - (20) INV_ERROR_SERIAL_CLOSED - * - (21) INV_ERROR_SERIAL_OPEN_ERROR - * - (22) INV_ERROR_SERIAL_READ - * - (23) INV_ERROR_SERIAL_WRITE - * - (24) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED - * - (25) INV_ERROR_SM_TRANSITION - * - (26) INV_ERROR_SM_IMPROPER_STATE - * - (30) INV_ERROR_FIFO_OVERFLOW - * - (31) INV_ERROR_FIFO_FOOTER - * - (32) INV_ERROR_FIFO_READ_COUNT - * - (33) INV_ERROR_FIFO_READ_DATA - * - (40) INV_ERROR_MEMORY_SET - * - (50) INV_ERROR_LOG_MEMORY_ERROR - * - (51) INV_ERROR_LOG_OUTPUT_ERROR - * - (60) INV_ERROR_OS_BAD_PTR - * - (61) INV_ERROR_OS_BAD_HANDLE - * - (62) INV_ERROR_OS_CREATE_FAILED - * - (63) INV_ERROR_OS_LOCK_FAILED - * - (70) INV_ERROR_COMPASS_DATA_OVERFLOW - * - (71) INV_ERROR_COMPASS_DATA_UNDERFLOW - * - (72) INV_ERROR_COMPASS_DATA_NOT_READY - * - (73) INV_ERROR_COMPASS_DATA_ERROR - * - (75) INV_ERROR_CALIBRATION_LOAD - * - (76) INV_ERROR_CALIBRATION_STORE - * - (77) INV_ERROR_CALIBRATION_LEN - * - (78) INV_ERROR_CALIBRATION_CHECKSUM - * - (79) INV_ERROR_ACCEL_DATA_OVERFLOW - * - (80) INV_ERROR_ACCEL_DATA_UNDERFLOW - * - (81) INV_ERROR_ACCEL_DATA_NOT_READY - * - (82) INV_ERROR_ACCEL_DATA_ERROR - * - * @{ - * @file mltypes.h - * @} - */ - -#ifndef MLTYPES_H -#define MLTYPES_H - -#ifdef __KERNEL__ -#include <linux/types.h> -#else -#include "stdint_invensense.h" -#endif - -/*--------------------------- - ML Types ----------------------------*/ - -/** - * @struct inv_error_t mltypes.h "mltypes" - * @brief The MPL Error Code return type. - * - * @code - * typedef unsigned char inv_error_t; - * @endcode - */ -typedef unsigned char inv_error_t; - -#ifndef __cplusplus -#ifndef __KERNEL__ -typedef int_fast8_t bool; -#endif -#endif - -/*--------------------------- - ML Defines ----------------------------*/ - -#ifndef NULL -#define NULL 0 -#endif - -#ifndef TRUE -#define TRUE 1 -#endif - -#ifndef FALSE -#define FALSE 0 -#endif - -#ifndef __KERNEL__ -#ifndef ARRAY_SIZE -/* Dimension of an array */ -#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0])) -#endif -#endif -/* - ML Errors. - */ -#define ERROR_NAME(x) (#x) -#define ERROR_CHECK_FIRST(first, x) \ - { if (INV_SUCCESS == first) first = x; } - -#define INV_SUCCESS (0) -/* Generic Error code. Proprietary Error Codes only */ -#define INV_ERROR (1) - -/* Compatibility and other generic error codes */ -#define INV_ERROR_INVALID_PARAMETER (2) -#define INV_ERROR_FEATURE_NOT_ENABLED (3) -#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (4) -#define INV_ERROR_DMP_NOT_STARTED (6) -#define INV_ERROR_DMP_STARTED (7) -#define INV_ERROR_NOT_OPENED (8) -#define INV_ERROR_OPENED (9) -#define INV_ERROR_INVALID_MODULE (10) -#define INV_ERROR_MEMORY_EXAUSTED (11) -#define INV_ERROR_DIVIDE_BY_ZERO (12) -#define INV_ERROR_ASSERTION_FAILURE (13) -#define INV_ERROR_FILE_OPEN (14) -#define INV_ERROR_FILE_READ (15) -#define INV_ERROR_FILE_WRITE (16) -#define INV_ERROR_INVALID_CONFIGURATION (17) - -/* Serial Communication */ -#define INV_ERROR_SERIAL_CLOSED (20) -#define INV_ERROR_SERIAL_OPEN_ERROR (21) -#define INV_ERROR_SERIAL_READ (22) -#define INV_ERROR_SERIAL_WRITE (23) -#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (24) - -/* SM = State Machine */ -#define INV_ERROR_SM_TRANSITION (25) -#define INV_ERROR_SM_IMPROPER_STATE (26) - -/* Fifo */ -#define INV_ERROR_FIFO_OVERFLOW (30) -#define INV_ERROR_FIFO_FOOTER (31) -#define INV_ERROR_FIFO_READ_COUNT (32) -#define INV_ERROR_FIFO_READ_DATA (33) - -/* Memory & Registers, Set & Get */ -#define INV_ERROR_MEMORY_SET (40) - -#define INV_ERROR_LOG_MEMORY_ERROR (50) -#define INV_ERROR_LOG_OUTPUT_ERROR (51) - -/* OS interface errors */ -#define INV_ERROR_OS_BAD_PTR (60) -#define INV_ERROR_OS_BAD_HANDLE (61) -#define INV_ERROR_OS_CREATE_FAILED (62) -#define INV_ERROR_OS_LOCK_FAILED (63) - -/* Compass errors */ -#define INV_ERROR_COMPASS_DATA_OVERFLOW (70) -#define INV_ERROR_COMPASS_DATA_UNDERFLOW (71) -#define INV_ERROR_COMPASS_DATA_NOT_READY (72) -#define INV_ERROR_COMPASS_DATA_ERROR (73) - -/* Load/Store calibration */ -#define INV_ERROR_CALIBRATION_LOAD (75) -#define INV_ERROR_CALIBRATION_STORE (76) -#define INV_ERROR_CALIBRATION_LEN (77) -#define INV_ERROR_CALIBRATION_CHECKSUM (78) - -/* Accel errors */ -#define INV_ERROR_ACCEL_DATA_OVERFLOW (79) -#define INV_ERROR_ACCEL_DATA_UNDERFLOW (80) -#define INV_ERROR_ACCEL_DATA_NOT_READY (81) -#define INV_ERROR_ACCEL_DATA_ERROR (82) - -#ifdef INV_USE_LEGACY_NAMES -#define ML_SUCCESS INV_SUCCESS -#define ML_ERROR INV_ERROR -#define ML_ERROR_INVALID_PARAMETER INV_ERROR_INVALID_PARAMETER -#define ML_ERROR_FEATURE_NOT_ENABLED INV_ERROR_FEATURE_NOT_ENABLED -#define ML_ERROR_FEATURE_NOT_IMPLEMENTED INV_ERROR_FEATURE_NOT_IMPLEMENTED -#define ML_ERROR_DMP_NOT_STARTED INV_ERROR_DMP_NOT_STARTED -#define ML_ERROR_DMP_STARTED INV_ERROR_DMP_STARTED -#define ML_ERROR_NOT_OPENED INV_ERROR_NOT_OPENED -#define ML_ERROR_OPENED INV_ERROR_OPENED -#define ML_ERROR_INVALID_MODULE INV_ERROR_INVALID_MODULE -#define ML_ERROR_MEMORY_EXAUSTED INV_ERROR_MEMORY_EXAUSTED -#define ML_ERROR_DIVIDE_BY_ZERO INV_ERROR_DIVIDE_BY_ZERO -#define ML_ERROR_ASSERTION_FAILURE INV_ERROR_ASSERTION_FAILURE -#define ML_ERROR_FILE_OPEN INV_ERROR_FILE_OPEN -#define ML_ERROR_FILE_READ INV_ERROR_FILE_READ -#define ML_ERROR_FILE_WRITE INV_ERROR_FILE_WRITE -#define ML_ERROR_INVALID_CONFIGURATION INV_ERROR_INVALID_CONFIGURATION -#define ML_ERROR_SERIAL_CLOSED INV_ERROR_SERIAL_CLOSED -#define ML_ERROR_SERIAL_OPEN_ERROR INV_ERROR_SERIAL_OPEN_ERROR -#define ML_ERROR_SERIAL_READ INV_ERROR_SERIAL_READ -#define ML_ERROR_SERIAL_WRITE INV_ERROR_SERIAL_WRITE -#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED \ - INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED -#define ML_ERROR_SM_TRANSITION INV_ERROR_SM_TRANSITION -#define ML_ERROR_SM_IMPROPER_STATE INV_ERROR_SM_IMPROPER_STATE -#define ML_ERROR_FIFO_OVERFLOW INV_ERROR_FIFO_OVERFLOW -#define ML_ERROR_FIFO_FOOTER INV_ERROR_FIFO_FOOTER -#define ML_ERROR_FIFO_READ_COUNT INV_ERROR_FIFO_READ_COUNT -#define ML_ERROR_FIFO_READ_DATA INV_ERROR_FIFO_READ_DATA -#define ML_ERROR_MEMORY_SET INV_ERROR_MEMORY_SET -#define ML_ERROR_LOG_MEMORY_ERROR INV_ERROR_LOG_MEMORY_ERROR -#define ML_ERROR_LOG_OUTPUT_ERROR INV_ERROR_LOG_OUTPUT_ERROR -#define ML_ERROR_OS_BAD_PTR INV_ERROR_OS_BAD_PTR -#define ML_ERROR_OS_BAD_HANDLE INV_ERROR_OS_BAD_HANDLE -#define ML_ERROR_OS_CREATE_FAILED INV_ERROR_OS_CREATE_FAILED -#define ML_ERROR_OS_LOCK_FAILED INV_ERROR_OS_LOCK_FAILED -#define ML_ERROR_COMPASS_DATA_OVERFLOW INV_ERROR_COMPASS_DATA_OVERFLOW -#define ML_ERROR_COMPASS_DATA_UNDERFLOW INV_ERROR_COMPASS_DATA_UNDERFLOW -#define ML_ERROR_COMPASS_DATA_NOT_READY INV_ERROR_COMPASS_DATA_NOT_READY -#define ML_ERROR_COMPASS_DATA_ERROR INV_ERROR_COMPASS_DATA_ERROR -#define ML_ERROR_CALIBRATION_LOAD INV_ERROR_CALIBRATION_LOAD -#define ML_ERROR_CALIBRATION_STORE INV_ERROR_CALIBRATION_STORE -#define ML_ERROR_CALIBRATION_LEN INV_ERROR_CALIBRATION_LEN -#define ML_ERROR_CALIBRATION_CHECKSUM INV_ERROR_CALIBRATION_CHECKSUM -#define ML_ERROR_ACCEL_DATA_OVERFLOW INV_ERROR_ACCEL_DATA_OVERFLOW -#define ML_ERROR_ACCEL_DATA_UNDERFLOW INV_ERROR_ACCEL_DATA_UNDERFLOW -#define ML_ERROR_ACCEL_DATA_NOT_READY INV_ERROR_ACCEL_DATA_NOT_READY -#define ML_ERROR_ACCEL_DATA_ERROR INV_ERROR_ACCEL_DATA_ERROR -#endif - -/* For Linux coding compliance */ -#ifndef __KERNEL__ -#define EXPORT_SYMBOL(x) -#endif - -/*--------------------------- - p-Types ----------------------------*/ - -#endif /* MLTYPES_H */ diff --git a/60xx/mlsdk/platform/include/mpu3050.h b/60xx/mlsdk/platform/include/mpu3050.h deleted file mode 100644 index c363a00..0000000 --- a/60xx/mlsdk/platform/include/mpu3050.h +++ /dev/null @@ -1,255 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef __MPU_H_ -#error Do not include this file directly. Include mpu.h instead. -#endif - -#ifndef __MPU3050_H_ -#define __MPU3050_H_ - -#ifdef __KERNEL__ -#include <linux/types.h> -#endif - -#if !defined CONFIG_MPU_SENSORS_MPU3050 -#error MPU6000 build including MPU3050 header -#endif - -#define MPU_NAME "mpu3050" -#define DEFAULT_MPU_SLAVEADDR 0x68 - -/*==== MPU REGISTER SET ====*/ -enum mpu_register { - MPUREG_WHO_AM_I = 0, /* 00 0x00 */ - MPUREG_PRODUCT_ID, /* 01 0x01 */ - MPUREG_02_RSVD, /* 02 0x02 */ - MPUREG_03_RSVD, /* 03 0x03 */ - MPUREG_04_RSVD, /* 04 0x04 */ - MPUREG_XG_OFFS_TC, /* 05 0x05 */ - MPUREG_06_RSVD, /* 06 0x06 */ - MPUREG_07_RSVD, /* 07 0x07 */ - MPUREG_YG_OFFS_TC, /* 08 0x08 */ - MPUREG_09_RSVD, /* 09 0x09 */ - MPUREG_0A_RSVD, /* 10 0x0a */ - MPUREG_ZG_OFFS_TC, /* 11 0x0b */ - MPUREG_X_OFFS_USRH, /* 12 0x0c */ - MPUREG_X_OFFS_USRL, /* 13 0x0d */ - MPUREG_Y_OFFS_USRH, /* 14 0x0e */ - MPUREG_Y_OFFS_USRL, /* 15 0x0f */ - MPUREG_Z_OFFS_USRH, /* 16 0x10 */ - MPUREG_Z_OFFS_USRL, /* 17 0x11 */ - MPUREG_FIFO_EN1, /* 18 0x12 */ - MPUREG_FIFO_EN2, /* 19 0x13 */ - MPUREG_AUX_SLV_ADDR, /* 20 0x14 */ - MPUREG_SMPLRT_DIV, /* 21 0x15 */ - MPUREG_DLPF_FS_SYNC, /* 22 0x16 */ - MPUREG_INT_CFG, /* 23 0x17 */ - MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */ - MPUREG_19_RSVD, /* 25 0x19 */ - MPUREG_INT_STATUS, /* 26 0x1a */ - MPUREG_TEMP_OUT_H, /* 27 0x1b */ - MPUREG_TEMP_OUT_L, /* 28 0x1c */ - MPUREG_GYRO_XOUT_H, /* 29 0x1d */ - MPUREG_GYRO_XOUT_L, /* 30 0x1e */ - MPUREG_GYRO_YOUT_H, /* 31 0x1f */ - MPUREG_GYRO_YOUT_L, /* 32 0x20 */ - MPUREG_GYRO_ZOUT_H, /* 33 0x21 */ - MPUREG_GYRO_ZOUT_L, /* 34 0x22 */ - MPUREG_23_RSVD, /* 35 0x23 */ - MPUREG_24_RSVD, /* 36 0x24 */ - MPUREG_25_RSVD, /* 37 0x25 */ - MPUREG_26_RSVD, /* 38 0x26 */ - MPUREG_27_RSVD, /* 39 0x27 */ - MPUREG_28_RSVD, /* 40 0x28 */ - MPUREG_29_RSVD, /* 41 0x29 */ - MPUREG_2A_RSVD, /* 42 0x2a */ - MPUREG_2B_RSVD, /* 43 0x2b */ - MPUREG_2C_RSVD, /* 44 0x2c */ - MPUREG_2D_RSVD, /* 45 0x2d */ - MPUREG_2E_RSVD, /* 46 0x2e */ - MPUREG_2F_RSVD, /* 47 0x2f */ - MPUREG_30_RSVD, /* 48 0x30 */ - MPUREG_31_RSVD, /* 49 0x31 */ - MPUREG_32_RSVD, /* 50 0x32 */ - MPUREG_33_RSVD, /* 51 0x33 */ - MPUREG_34_RSVD, /* 52 0x34 */ - MPUREG_DMP_CFG_1, /* 53 0x35 */ - MPUREG_DMP_CFG_2, /* 54 0x36 */ - MPUREG_BANK_SEL, /* 55 0x37 */ - MPUREG_MEM_START_ADDR, /* 56 0x38 */ - MPUREG_MEM_R_W, /* 57 0x39 */ - MPUREG_FIFO_COUNTH, /* 58 0x3a */ - MPUREG_FIFO_COUNTL, /* 59 0x3b */ - MPUREG_FIFO_R_W, /* 60 0x3c */ - MPUREG_USER_CTRL, /* 61 0x3d */ - MPUREG_PWR_MGM, /* 62 0x3e */ - MPUREG_3F_RSVD, /* 63 0x3f */ - NUM_OF_MPU_REGISTERS /* 64 0x40 */ -}; - -/*==== BITS FOR MPU ====*/ - -/*---- MPU 'FIFO_EN1' register (12) ----*/ -#define BIT_TEMP_OUT 0x80 -#define BIT_GYRO_XOUT 0x40 -#define BIT_GYRO_YOUT 0x20 -#define BIT_GYRO_ZOUT 0x10 -#define BIT_ACCEL_XOUT 0x08 -#define BIT_ACCEL_YOUT 0x04 -#define BIT_ACCEL_ZOUT 0x02 -#define BIT_AUX_1OUT 0x01 -/*---- MPU 'FIFO_EN2' register (13) ----*/ -#define BIT_AUX_2OUT 0x02 -#define BIT_AUX_3OUT 0x01 -/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/ -#define BITS_EXT_SYNC_NONE 0x00 -#define BITS_EXT_SYNC_TEMP 0x20 -#define BITS_EXT_SYNC_GYROX 0x40 -#define BITS_EXT_SYNC_GYROY 0x60 -#define BITS_EXT_SYNC_GYROZ 0x80 -#define BITS_EXT_SYNC_ACCELX 0xA0 -#define BITS_EXT_SYNC_ACCELY 0xC0 -#define BITS_EXT_SYNC_ACCELZ 0xE0 -#define BITS_EXT_SYNC_MASK 0xE0 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 -#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 -#define BITS_DLPF_CFG_188HZ 0x01 -#define BITS_DLPF_CFG_98HZ 0x02 -#define BITS_DLPF_CFG_42HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 -#define BITS_DLPF_CFG_MASK 0x07 -/*---- MPU 'INT_CFG' register (17) ----*/ -#define BIT_ACTL 0x80 -#define BIT_ACTL_LOW 0x80 -#define BIT_ACTL_HIGH 0x00 -#define BIT_OPEN 0x40 -#define BIT_OPEN_DRAIN 0x40 -#define BIT_PUSH_PULL 0x00 -#define BIT_LATCH_INT_EN 0x20 -#define BIT_INT_PULSE_WIDTH_50US 0x00 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_INT_STAT_READ_2CLEAR 0x00 -#define BIT_MPU_RDY_EN 0x04 -#define BIT_DMP_INT_EN 0x02 -#define BIT_RAW_RDY_EN 0x01 -/*---- MPU 'INT_STATUS' register (1A) ----*/ -#define BIT_INT_STATUS_FIFO_OVERLOW 0x80 -#define BIT_MPU_RDY 0x04 -#define BIT_DMP_INT 0x02 -#define BIT_RAW_RDY 0x01 -/*---- MPU 'BANK_SEL' register (37) ----*/ -#define BIT_PRFTCH_EN 0x20 -#define BIT_CFG_USER_BANK 0x10 -#define BITS_MEM_SEL 0x0f -/*---- MPU 'USER_CTRL' register (3D) ----*/ -#define BIT_DMP_EN 0x80 -#define BIT_FIFO_EN 0x40 -#define BIT_AUX_IF_EN 0x20 -#define BIT_AUX_RD_LENG 0x10 -#define BIT_AUX_IF_RST 0x08 -#define BIT_DMP_RST 0x04 -#define BIT_FIFO_RST 0x02 -#define BIT_GYRO_RST 0x01 -/*---- MPU 'PWR_MGM' register (3E) ----*/ -#define BIT_H_RESET 0x80 -#define BIT_SLEEP 0x40 -#define BIT_STBY_XG 0x20 -#define BIT_STBY_YG 0x10 -#define BIT_STBY_ZG 0x08 -#define BITS_CLKSEL 0x07 - -/*---- MPU Silicon Revision ----*/ -#define MPU_SILICON_REV_A4 1 /* MPU A4 Device */ -#define MPU_SILICON_REV_B1 2 /* MPU B1 Device */ -#define MPU_SILICON_REV_B4 3 /* MPU B4 Device */ -#define MPU_SILICON_REV_B6 4 /* MPU B6 Device */ - -/*---- MPU Memory ----*/ -#define MPU_MEM_BANK_SIZE (256) -#define FIFO_HW_SIZE (512) - -enum MPU_MEMORY_BANKS { - MPU_MEM_RAM_BANK_0 = 0, - MPU_MEM_RAM_BANK_1, - MPU_MEM_RAM_BANK_2, - MPU_MEM_RAM_BANK_3, - MPU_MEM_NUM_RAM_BANKS, - MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS, - /* This one is always last */ - MPU_MEM_NUM_BANKS -}; - -/*---- structure containing control variables used by MLDL ----*/ -/*---- MPU clock source settings ----*/ -/*---- MPU filter selections ----*/ -enum mpu_filter { - MPU_FILTER_256HZ_NOLPF2 = 0, - MPU_FILTER_188HZ, - MPU_FILTER_98HZ, - MPU_FILTER_42HZ, - MPU_FILTER_20HZ, - MPU_FILTER_10HZ, - MPU_FILTER_5HZ, - MPU_FILTER_2100HZ_NOLPF, - NUM_MPU_FILTER -}; - -enum mpu_fullscale { - MPU_FS_250DPS = 0, - MPU_FS_500DPS, - MPU_FS_1000DPS, - MPU_FS_2000DPS, - NUM_MPU_FS -}; - -enum mpu_clock_sel { - MPU_CLK_SEL_INTERNAL = 0, - MPU_CLK_SEL_PLLGYROX, - MPU_CLK_SEL_PLLGYROY, - MPU_CLK_SEL_PLLGYROZ, - MPU_CLK_SEL_PLLEXT32K, - MPU_CLK_SEL_PLLEXT19M, - MPU_CLK_SEL_RESERVED, - MPU_CLK_SEL_STOP, - NUM_CLK_SEL -}; - -enum mpu_ext_sync { - MPU_EXT_SYNC_NONE = 0, - MPU_EXT_SYNC_TEMP, - MPU_EXT_SYNC_GYROX, - MPU_EXT_SYNC_GYROY, - MPU_EXT_SYNC_GYROZ, - MPU_EXT_SYNC_ACCELX, - MPU_EXT_SYNC_ACCELY, - MPU_EXT_SYNC_ACCELZ, - NUM_MPU_EXT_SYNC -}; - -#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \ - ((ext_sync << 5) | (full_scale << 3) | lpf) - -#endif /* __MPU3050_H_ */ diff --git a/60xx/mlsdk/platform/include/stdint_invensense.h b/60xx/mlsdk/platform/include/stdint_invensense.h deleted file mode 100644 index d238813..0000000 --- a/60xx/mlsdk/platform/include/stdint_invensense.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -#ifndef STDINT_INVENSENSE_H -#define STDINT_INVENSENSE_H - -#ifndef WIN32 - -#ifdef __KERNEL__ -#include <linux/types.h> -#else -#include <stdint.h> -#endif - -#else - -#include <windows.h> - -typedef char int8_t; -typedef short int16_t; -typedef long int32_t; - -typedef unsigned char uint8_t; -typedef unsigned short uint16_t; -typedef unsigned long uint32_t; - -typedef int int_fast8_t; -typedef int int_fast16_t; -typedef long int_fast32_t; - -typedef unsigned int uint_fast8_t; -typedef unsigned int uint_fast16_t; -typedef unsigned long uint_fast32_t; - -#endif - -#endif // STDINT_INVENSENSE_H diff --git a/60xx/mlsdk/platform/linux/kernel/mpuirq.h b/60xx/mlsdk/platform/linux/kernel/mpuirq.h deleted file mode 100644 index 352170b..0000000 --- a/60xx/mlsdk/platform/linux/kernel/mpuirq.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef __MPUIRQ__ -#define __MPUIRQ__ - -#ifdef __KERNEL__ -#include <linux/time.h> -#include <linux/ioctl.h> -#include "mldl_cfg.h" -#else -#include <sys/ioctl.h> -#include <sys/time.h> -#endif - -#define MPUIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x40, unsigned long) -#define MPUIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x41, unsigned long) -#define MPUIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x42, struct timeval) -#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long) - -#ifdef __KERNEL__ -void mpuirq_exit(void); -int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg); -#endif - -#endif diff --git a/60xx/mlsdk/platform/linux/kernel/slaveirq.h b/60xx/mlsdk/platform/linux/kernel/slaveirq.h deleted file mode 100644 index beb352b..0000000 --- a/60xx/mlsdk/platform/linux/kernel/slaveirq.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef __SLAVEIRQ__ -#define __SLAVEIRQ__ - -#include <linux/mpu.h> -#include "mpuirq.h" - -#define SLAVEIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x50, unsigned long) -#define SLAVEIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x51, unsigned long) -#define SLAVEIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x52, unsigned long) - - -void slaveirq_exit(struct ext_slave_platform_data *pdata); -int slaveirq_init(struct i2c_adapter *slave_adapter, - struct ext_slave_platform_data *pdata, char *name); - - -#endif diff --git a/60xx/mlsdk/platform/linux/kernel/timerirq.h b/60xx/mlsdk/platform/linux/kernel/timerirq.h deleted file mode 100644 index dc3eea2..0000000 --- a/60xx/mlsdk/platform/linux/kernel/timerirq.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -#ifndef __TIMERIRQ__ -#define __TIMERIRQ__ - -#include <linux/mpu.h> - -#define TIMERIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x60, unsigned long) -#define TIMERIRQ_GET_INTERRUPT_CNT _IOW(MPU_IOCTL, 0x61, unsigned long) -#define TIMERIRQ_START _IOW(MPU_IOCTL, 0x62, unsigned long) -#define TIMERIRQ_STOP _IO(MPU_IOCTL, 0x63) - -#endif diff --git a/60xx/mlsdk/platform/linux/log_linux.c b/60xx/mlsdk/platform/linux/log_linux.c deleted file mode 100644 index 8e75753..0000000 --- a/60xx/mlsdk/platform/linux/log_linux.c +++ /dev/null @@ -1,114 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/****************************************************************************** - * $Id: log_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ - ******************************************************************************/ - -/** - * @defgroup MPL_LOG - * @brief Logging facility for the MPL - * - * @{ - * @file log.c - * @brief Core logging facility functions. - * - * -**/ - -#include <stdio.h> -#include <string.h> -#include "log.h" -#include "mltypes.h" - -#define LOG_BUFFER_SIZE (256) - -#ifdef WIN32 -#define snprintf _snprintf -#define vsnprintf _vsnprintf -#endif - -int _MLPrintLog (int priority, const char* tag, const char* fmt, ...) -{ - va_list ap; - int result; - - va_start(ap, fmt); - result = _MLPrintVaLog(priority,tag,fmt,ap); - va_end(ap); - - return result; -} - -int _MLPrintVaLog(int priority, const char* tag, const char* fmt, va_list args) -{ - int result; - char buf[LOG_BUFFER_SIZE]; - char new_fmt[LOG_BUFFER_SIZE]; - char priority_char; - - if (NULL == fmt) { - fmt = ""; - } - - switch (priority) { - case MPL_LOG_UNKNOWN: - priority_char = 'U'; - break; - case MPL_LOG_VERBOSE: - priority_char = 'V'; - break; - case MPL_LOG_DEBUG: - priority_char = 'D'; - break; - case MPL_LOG_INFO: - priority_char = 'I'; - break; - case MPL_LOG_WARN: - priority_char = 'W'; - break; - case MPL_LOG_ERROR: - priority_char = 'E'; - break; - case MPL_LOG_SILENT: - priority_char = 'S'; - break; - case MPL_LOG_DEFAULT: - default: - priority_char = 'D'; - break; - }; - - result = snprintf(new_fmt, sizeof(new_fmt), "%c/%s:%s", - priority_char, tag, fmt); - if (result <= 0) { - return INV_ERROR_LOG_MEMORY_ERROR; - } - result = vsnprintf(buf,sizeof(buf),new_fmt, args); - if (result <= 0) { - return INV_ERROR_LOG_OUTPUT_ERROR; - } - - result = _MLWriteLog(buf, strlen(buf)); - return INV_SUCCESS; -} - -/** - * @} -**/ - - diff --git a/60xx/mlsdk/platform/linux/log_printf_linux.c b/60xx/mlsdk/platform/linux/log_printf_linux.c deleted file mode 100644 index 744a223..0000000 --- a/60xx/mlsdk/platform/linux/log_printf_linux.c +++ /dev/null @@ -1,43 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: log_printf_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - ******************************************************************************/ - -/** - * @addtogroup MPL_LOG - * - * @{ - * @file log_printf.c - * @brief printf replacement for _MLWriteLog. - */ - -#include <stdio.h> -#include "log.h" - -int _MLWriteLog (const char * buf, int buflen) -{ - return fputs(buf, stdout); -} - -/** - * @} - */ - diff --git a/60xx/mlsdk/platform/linux/mlos_linux.c b/60xx/mlsdk/platform/linux/mlos_linux.c deleted file mode 100644 index fd3b002..0000000 --- a/60xx/mlsdk/platform/linux/mlos_linux.c +++ /dev/null @@ -1,206 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ -/******************************************************************************* - * - * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -/** - * @defgroup MLOS - * @brief OS Interface. - * - * @{ - * @file mlos.c - * @brief OS Interface. -**/ - -/* ------------- */ -/* - Includes. - */ -/* ------------- */ - -#include <sys/time.h> -#include <unistd.h> -#include <pthread.h> -#include <stdlib.h> - -#include "stdint_invensense.h" - -#include "mlos.h" -#include <errno.h> - - -/* -------------- */ -/* - Functions. - */ -/* -------------- */ - -/** - * @brief Allocate space - * @param numBytes number of bytes - * @return pointer to allocated space -**/ -void *inv_malloc(unsigned int numBytes) -{ - // Allocate space. - void *allocPtr = malloc(numBytes); - return allocPtr; -} - - -/** - * @brief Free allocated space - * @param ptr pointer to space to deallocate - * @return error code. -**/ -inv_error_t inv_free(void *ptr) -{ - // Deallocate space. - free(ptr); - - return INV_SUCCESS; -} - - -/** - * @brief Mutex create function - * @param mutex pointer to mutex handle - * @return error code. - */ -inv_error_t inv_create_mutex(HANDLE *mutex) -{ - int res; - pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); - if(pm == NULL) - return INV_ERROR; - - res = pthread_mutex_init(pm, NULL); - if(res == -1) { - free(pm); - return INV_ERROR_OS_CREATE_FAILED; - } - - *mutex = (HANDLE)pm; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex lock function - * @param mutex Mutex handle - * @return error code. - */ -inv_error_t inv_lock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t*)mutex; - - res = pthread_mutex_lock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief Mutex unlock function - * @param mutex mutex handle - * @return error code. -**/ -inv_error_t inv_unlock_mutex(HANDLE mutex) -{ - int res; - pthread_mutex_t *pm = (pthread_mutex_t*)mutex; - - res = pthread_mutex_unlock(pm); - if(res == -1) - return INV_ERROR_OS_LOCK_FAILED; - - return INV_SUCCESS; -} - - -/** - * @brief open file - * @param filename name of the file to open. - * @return error code. - */ -FILE *inv_fopen(char *filename) -{ - FILE *fp = fopen(filename,"r"); - return fp; -} - - -/** - * @brief close the file. - * @param fp handle to file to close. - * @return error code. - */ -void inv_fclose(FILE *fp) -{ - fclose(fp); -} - -/** - * @brief Close Handle - * @param handle handle to the resource. - * @return Zero if success, an error code otherwise. - */ -inv_error_t inv_destroy_mutex(HANDLE handle) -{ - int error; - pthread_mutex_t *pm = (pthread_mutex_t*)handle; - error = pthread_mutex_destroy(pm); - if (error) { - return errno; - } - free((void*) handle); - - return INV_SUCCESS;} - - -/** - * @brief Sleep function. - */ -void inv_sleep(int mSecs) -{ - usleep(mSecs*1000); -} - - -/** - * @brief get system's internal tick count. - * Used for time reference. - * @return current tick count. - */ -unsigned long inv_get_tick_count() -{ - struct timeval tv; - - if (gettimeofday(&tv, NULL) !=0) - return 0; - - return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); -} - - /**********************/ - /** @} */ /* defgroup */ -/**********************/ - diff --git a/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c deleted file mode 100644 index 1bd71de..0000000 --- a/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c +++ /dev/null @@ -1,497 +0,0 @@ -/* - $License: - Copyright 2011 InvenSense, Inc. - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. - $ - */ - -/****************************************************************************** - * $Id: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $ - *****************************************************************************/ - -/** - * @defgroup MLSL (Motion Library - Serial Layer) - * @brief The Motion Library System Layer provides the Motion Library the - * interface to the system functions. - * - * @{ - * @file mlsl_linux_mpu.c - * @brief The Motion Library System Layer. - * - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ -#include <stdio.h> -#include <sys/ioctl.h> -#include <stdlib.h> -#include <fcntl.h> -#include <errno.h> -#include <unistd.h> -#include <linux/fs.h> -#include <linux/i2c.h> -#include <string.h> -#include <signal.h> -#include <time.h> - -#include "mpu.h" -#if defined CONFIG_MPU_SENSORS_MPU6050A2 -# include "mpu6050a2.h" -#elif defined CONFIG_MPU_SENSORS_MPU6050B1 -# include "mpu6050b1.h" -#elif defined CONFIG_MPU_SENSORS_MPU3050 -# include "mpu3050.h" -#else -#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx -#endif - -#include "mlsl.h" -#include "mlos.h" -#include "mlmath.h" -#include "mlinclude.h" - -#define MLCAL_ID (0x0A0B0C0DL) -#define MLCAL_FILE "/data/cal.bin" -#define MLCFG_ID (0x01020304L) -#define MLCFG_FILE "/data/cfg.bin" - -#include <log.h> -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mlsl" - -#ifndef I2CDEV -#define I2CDEV "/dev/mpu" -#endif - -#define SERIAL_FULL_DEBUG (0) - -/* --------------- */ -/* - Prototypes. - */ -/* --------------- */ - -/* ----------------------- */ -/* - Function Pointers. - */ -/* ----------------------- */ - -/* --------------------------- */ -/* - Global and Static vars. - */ -/* --------------------------- */ - -/* ---------------- */ -/* - Definitions. - */ -/* ---------------- */ - -inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len) -{ - FILE *fp; - int bytesRead; - - fp = fopen(MLCFG_FILE, "rb"); - if (fp == NULL) { - MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE); - return INV_ERROR_FILE_OPEN; - } - bytesRead = fread(cfg, 1, len, fp); - if (bytesRead != len) { - MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", - bytesRead, len); - return INV_ERROR_FILE_READ; - } - fclose(fp); - - if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3]) - != MLCFG_ID) { - return INV_ERROR_ASSERTION_FAILURE; - } - - return INV_SUCCESS; -} - -inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len) -{ - FILE *fp; - int bytesWritten; - unsigned char cfgId[4]; - - fp = fopen(MLCFG_FILE,"wb"); - if (fp == NULL) { - MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE); - return INV_ERROR_FILE_OPEN; - } - - cfgId[0] = (unsigned char)(MLCFG_ID >> 24); - cfgId[1] = (unsigned char)(MLCFG_ID >> 16); - cfgId[2] = (unsigned char)(MLCFG_ID >> 8); - cfgId[3] = (unsigned char)(MLCFG_ID); - bytesWritten = fwrite(cfgId, 1, 4, fp); - if (bytesWritten != 4) { - MPL_LOGE("CFG ID could not be written on file\n"); - return INV_ERROR_FILE_WRITE; - } - - bytesWritten = fwrite(cfg, 1, len, fp); - if (bytesWritten != len) { - MPL_LOGE("bytes write (%d) don't match requested length (%d)\n", - bytesWritten, len); - return INV_ERROR_FILE_WRITE; - } - - fclose(fp); - - return INV_SUCCESS; -} - -inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len) -{ - FILE *fp; - int bytesRead; - inv_error_t result = INV_SUCCESS; - - fp = fopen(MLCAL_FILE,"rb"); - if (fp == NULL) { - MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - bytesRead = fread(cal, 1, len, fp); - if (bytesRead != len) { - MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", - bytesRead, len); - result = INV_ERROR_FILE_READ; - goto read_cal_end; - } - - /* MLCAL_ID not used - if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3]) - != MLCAL_ID) { - result = INV_ERROR_ASSERTION_FAILURE; - goto read_cal_end; - } - */ -read_cal_end: - fclose(fp); - return result; -} - -inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len) -{ - FILE *fp; - int bytesWritten; - inv_error_t result = INV_SUCCESS; - - fp = fopen(MLCAL_FILE,"wb"); - if (fp == NULL) { - MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - bytesWritten = fwrite(cal, 1, len, fp); - if (bytesWritten != len) { - MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", - bytesWritten, len); - result = INV_ERROR_FILE_WRITE; - } - fclose(fp); - return result; -} - -inv_error_t inv_serial_get_cal_length(unsigned int *len) -{ - FILE *calFile; - *len = 0; - - calFile = fopen(MLCAL_FILE, "rb"); - if (calFile == NULL) { - MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); - return INV_ERROR_FILE_OPEN; - } - - *len += (unsigned char)fgetc(calFile) << 24; - *len += (unsigned char)fgetc(calFile) << 16; - *len += (unsigned char)fgetc(calFile) << 8; - *len += (unsigned char)fgetc(calFile); - - fclose(calFile); - - if (*len <= 0) - return INV_ERROR_FILE_READ; - - return INV_SUCCESS; -} - -inv_error_t inv_serial_open(char const *port, void **sl_handle) -{ - INVENSENSE_FUNC_START; - - if (NULL == port) { - port = I2CDEV; - } - *sl_handle = (void*)(uintptr_t) open(port, O_RDWR); - if((intptr_t)*sl_handle < 0) { - /* ERROR HANDLING; you can check errno to see what went wrong */ - MPL_LOGE("inv_serial_open\n"); - MPL_LOGE("I2C Error %d: Cannot open Adapter %s\n", errno, port); - return INV_ERROR_SERIAL_OPEN_ERROR; - } else { - MPL_LOGI("inv_serial_open: %s\n", port); - } - - return INV_SUCCESS; -} - -inv_error_t inv_serial_close(void *sl_handle) -{ - INVENSENSE_FUNC_START; - - close((int)(uintptr_t)sl_handle); - - return INV_SUCCESS; -} - -inv_error_t inv_serial_reset(void *sl_handle) -{ - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; -} - -inv_error_t inv_serial_single_write(void *sl_handle, - unsigned char slaveAddr, - unsigned char registerAddr, - unsigned char data) -{ - unsigned char buf[2]; - buf[0] = registerAddr; - buf[1] = data; - return inv_serial_write(sl_handle, slaveAddr, 2, buf); -} - -inv_error_t inv_serial_write(void *sl_handle, - unsigned char slaveAddr, - unsigned short length, - unsigned char const *data) -{ - INVENSENSE_FUNC_START; - struct mpu_read_write msg; - inv_error_t result; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - msg.address = 0; /* not used */ - msg.length = length; - msg.data = (unsigned char*)data; - - if ((result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE, &msg))) { - MPL_LOGE("I2C Error: could not write: R:%02x L:%d %d \n", - data[0], length, result); - return result; - } else if (SERIAL_FULL_DEBUG) { - char data_buff[4096] = ""; - char strchar[3]; - int ii; - for (ii = 0; ii < length; ii++) { - snprintf(strchar, sizeof(strchar), "%02x", data[0]); - strncat(data_buff, strchar, sizeof(data_buff)); - } - MPL_LOGI("I2C Write Success %02x %02x: %s \n", - data[0], length, data_buff); - } - - return INV_SUCCESS; -} - -inv_error_t inv_serial_read(void *sl_handle, - unsigned char slaveAddr, - unsigned char registerAddr, - unsigned short length, - unsigned char *data) -{ - INVENSENSE_FUNC_START; - int result = INV_SUCCESS; - struct mpu_read_write msg; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - msg.address = registerAddr; - msg.length = length; - msg.data = data; - - result = ioctl((int)(uintptr_t)sl_handle, MPU_READ, &msg); - - if (result != INV_SUCCESS) { - MPL_LOGE("I2C Error %08x: could not read: R:%02x L:%d\n", - result, registerAddr, length); - result = INV_ERROR_SERIAL_READ; - } else if (SERIAL_FULL_DEBUG) { - char data_buff[4096] = ""; - char strchar[3]; - int ii; - for (ii = 0; ii < length; ii++) { - snprintf(strchar, sizeof(strchar), "%02x", data[0]); - strncat(data_buff, strchar, sizeof(data_buff)); - } - MPL_LOGI("I2C Read Success %02x %02x: %s \n", - registerAddr, length, data_buff); - } - - return (inv_error_t) result; -} - -inv_error_t inv_serial_write_mem(void *sl_handle, - unsigned char mpu_addr, - unsigned short memAddr, - unsigned short length, - const unsigned char *data) -{ - INVENSENSE_FUNC_START; - struct mpu_read_write msg; - int result; - - msg.address = memAddr; - msg.length = length; - msg.data = (unsigned char *)data; - - result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_MEM, &msg); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else if (SERIAL_FULL_DEBUG) { - char data_buff[4096] = ""; - char strchar[3]; - int ii; - for (ii = 0; ii < length; ii++) { - snprintf(strchar, sizeof(strchar), "%02x", data[0]); - strncat(data_buff, strchar, sizeof(data_buff)); - } - MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n", - memAddr, length, data_buff); - } - return INV_SUCCESS; -} - -inv_error_t inv_serial_read_mem(void *sl_handle, - unsigned char mpu_addr, - unsigned short memAddr, - unsigned short length, - unsigned char *data) -{ - INVENSENSE_FUNC_START; - struct mpu_read_write msg; - int result; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - msg.address = memAddr; - msg.length = length; - msg.data = data; - - result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_MEM, &msg); - if (result != INV_SUCCESS) { - MPL_LOGE("I2C Error %08x: could not read memory: A:%04x L:%d\n", - result, memAddr, length); - return INV_ERROR_SERIAL_READ; - } else if (SERIAL_FULL_DEBUG) { - char data_buff[4096] = ""; - char strchar[3]; - int ii; - for (ii = 0; ii < length; ii++) { - snprintf(strchar, sizeof(strchar), "%02x", data[0]); - strncat(data_buff, strchar, sizeof(data_buff)); - } - MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n", - memAddr, length, data_buff); - } - return INV_SUCCESS; -} - -inv_error_t inv_serial_write_fifo(void *sl_handle, - unsigned char mpu_addr, - unsigned short length, - const unsigned char *data) -{ - INVENSENSE_FUNC_START; - struct mpu_read_write msg; - int result; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - msg.address = 0; /* Not used */ - msg.length = length; - msg.data = (unsigned char *)data; - - result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_FIFO, &msg); - if (result != INV_SUCCESS) { - MPL_LOGE("I2C Error: could not write fifo: %02x %02x\n", - MPUREG_FIFO_R_W, length); - return INV_ERROR_SERIAL_WRITE; - } else if (SERIAL_FULL_DEBUG) { - char data_buff[4096] = ""; - char strchar[3]; - int ii; - for (ii = 0; ii < length; ii++) { - snprintf(strchar, sizeof(strchar), "%02x", data[0]); - strncat(data_buff, strchar, sizeof(data_buff)); - } - MPL_LOGI("I2C Write Success %02x %02x: %s\n", - MPUREG_FIFO_R_W, length, data_buff); - } - return (inv_error_t) result; -} - -inv_error_t inv_serial_read_fifo(void *sl_handle, - unsigned char mpu_addr, - unsigned short length, - unsigned char *data) -{ - INVENSENSE_FUNC_START; - struct mpu_read_write msg; - int result; - - if (NULL == data) { - return INV_ERROR_INVALID_PARAMETER; - } - - msg.address = MPUREG_FIFO_R_W; /* Not used */ - msg.length = length; - msg.data = data; - - result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_FIFO, &msg); - if (result != INV_SUCCESS) { - MPL_LOGE("I2C Error %08x: could not read fifo: R:%02x L:%d\n", - result, MPUREG_FIFO_R_W, length); - return INV_ERROR_SERIAL_READ; - } else if (SERIAL_FULL_DEBUG) { - char data_buff[4096] = ""; - char strchar[3]; - int ii; - for (ii = 0; ii < length; ii++) { - snprintf(strchar, sizeof(strchar), "%02x", data[0]); - strncat(data_buff, strchar, sizeof(data_buff)); - } - MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n", - MPUREG_FIFO_R_W, length, data_buff); - } - return INV_SUCCESS; -} - -/** - * @} - */ - - @@ -2,14 +2,7 @@ ifneq ($(filter hammerhead, $(TARGET_DEVICE)),) # hammerhead expects 65xx sensors. include $(call all-named-subdir-makefiles,65xx) -else -ifneq ($(filter guppy dory, $(TARGET_DEVICE)),) +else ifneq ($(filter guppy dory, $(TARGET_DEVICE)),) # dory and guppy expect 6515 sensors. include $(call all-named-subdir-makefiles,6515) -else -ifneq ($(filter manta grouper tuna mako, $(TARGET_DEVICE)),) -# manta, grouper, tuna, and mako expect 60xx sensors. -include $(call all-named-subdir-makefiles,60xx) -endif -endif endif |