From 33ce91b37062fa63af192f5643de93f3beebe854 Mon Sep 17 00:00:00 2001 From: JP Abgrall Date: Wed, 3 Oct 2012 20:16:57 -0700 Subject: MotionApps 5.1.1 release, with MA 5.1.0 for further merge review. 1. Removed all #ifdef in HAL's member APIs. 2. Added necessary comments as reference. 3. Made changes for coding style, optimization and so on per prior comments. 4. Now raw/calibrated gyroscope sensors could co-exist Default sensor would be calibrated gyroscope sensor for getDefaultSensor() call in Android. * Correctly handle onPower()/masterEnable(). * Use the support functions for reading/writing sysfs. 1 line instead of 9 all over the place. * Fix return code for {read,write}_sysfs_int(): was > 0 in case of failure instead of < 0. Bug: 7211625 Change-Id: Ib49dab8ca0f48f45a2838de72f4f8ac011d0e68f --- libsensors_iio/CompassSensor.IIO.9150.cpp | 75 +- libsensors_iio/InputEventReader.h | 28 +- libsensors_iio/MPLSensor.cpp | 1136 ++++++++----- libsensors_iio/MPLSensor.h | 80 +- libsensors_iio/MPLSupport.cpp | 63 +- libsensors_iio/libmllite.so | Bin 151456 -> 90020 bytes libsensors_iio/libmplmpu.so | Bin 310092 -> 168684 bytes libsensors_iio/local_log_def.h | 35 +- libsensors_iio/sensor_params.h | 56 +- libsensors_iio/sensors.h | 23 +- libsensors_iio/sensors_mpl.cpp | 47 +- libsensors_iio/software/build/android/shared.mk | 3 +- .../software/core/HAL/include/inv_sysfs_utils.h | 83 - libsensors_iio/software/core/HAL/include/mlos.h | 104 -- .../software/core/HAL/linux/inv_sysfs_utils.c | 317 ---- .../software/core/HAL/linux/mlos_linux.c | 194 --- .../software/core/driver/include/linux/mpu.h | 249 +-- libsensors_iio/software/core/driver/include/log.h | 6 +- libsensors_iio/software/core/mllite/CMakeLists.txt | 39 - .../software/core/mllite/Engineering.cmake | 150 -- .../core/mllite/build/android/libmllite.so | Bin 0 -> 90020 bytes .../software/core/mllite/build/android/static.mk | 110 -- libsensors_iio/software/core/mllite/data_builder.c | 186 +-- libsensors_iio/software/core/mllite/data_builder.h | 8 + libsensors_iio/software/core/mllite/dmpconfig.txt | 3 - libsensors_iio/software/core/mllite/hal_outputs.c | 900 +++++----- libsensors_iio/software/core/mllite/hal_outputs.h | 2 + .../software/core/mllite/linux/inv_sysfs_utils.c | 318 ---- .../software/core/mllite/linux/inv_sysfs_utils.h | 84 - .../software/core/mllite/linux/ml_load_dmp.c | 62 +- .../software/core/mllite/linux/ml_stored_data.c | 53 +- .../software/core/mllite/linux/ml_stored_data.h | 2 +- .../software/core/mllite/linux/ml_sysfs_helper.c | 13 +- .../software/core/mllite/linux/ml_sysfs_helper.h | 4 +- .../software/core/mllite/linux/mlos_linux.c | 4 +- libsensors_iio/software/core/mllite/ml_math_func.c | 51 +- libsensors_iio/software/core/mllite/ml_math_func.h | 12 + libsensors_iio/software/core/mllite/mlmath.c | 68 - libsensors_iio/software/core/mllite/mpl.c | 2 +- .../software/core/mllite/results_holder.c | 2 +- .../software/core/mllite/storage_manager.c | 1 + libsensors_iio/software/core/mpl/adv_func.h | 30 - .../software/core/mpl/auto_calibration.h | 33 - .../software/core/mpl/build/android/libmplmpu.so | Bin 130934 -> 168684 bytes .../software/core/mpl/build/android/static.mk | 110 -- libsensors_iio/software/core/mpl/fusion_9axis.h | 2 + libsensors_iio/software/core/mpl/interpolator.h | 103 -- libsensors_iio/software/core/mpl/inv_log.h | 7 - libsensors_iio/software/core/mpl/invensense_adv.h | 1 + libsensors_iio/software/core/mpl/mlsetinterrupts.h | 23 - .../software/core/mpl/mlsupervisor_9axis.h | 57 - libsensors_iio/software/core/mpl/orientation.h | 42 - .../software/core/mpl/progressive_no_motion.h | 39 - .../software/core/mpl/quat_accuracy_monitor.h | 1 + libsensors_iio/software/core/mpl/sensor_moments.h | 42 - libsensors_iio/software/core/mpl/shake.h | 94 ++ libsensors_iio/software/core/mpl/state_storage.h | 25 - .../simple_apps/common/external_hardware.h | 156 -- .../software/simple_apps/common/fopenCMake.c | 56 - .../software/simple_apps/common/fopenCMake.h | 21 - .../software/simple_apps/common/gestureMenu.c | 725 -------- .../software/simple_apps/common/gestureMenu.h | 75 - .../software/simple_apps/common/helper.c | 110 -- .../software/simple_apps/common/helper.h | 103 -- .../software/simple_apps/common/mlerrorcode.c | 96 -- .../software/simple_apps/common/mlerrorcode.h | 86 - .../software/simple_apps/common/mlsetup.c | 1722 -------------------- .../software/simple_apps/common/mlsetup.h | 52 - libsensors_iio/software/simple_apps/common/slave.h | 176 -- .../console/linux/build/android/consoledmp-shared | Bin 23672 -> 0 bytes .../console/linux/build/android/shared.mk | 109 -- .../simple_apps/console/linux/build/filelist.mk | 23 - .../simple_apps/console/linux/console_test.c | 742 --------- .../input_sub/build/android/input_gyro-shared | Bin 16548 -> 0 bytes .../simple_apps/input_sub/build/android/shared.mk | 109 -- .../simple_apps/input_sub/build/filelist.mk | 13 - .../simple_apps/input_sub/test_input_gyro.c | 485 ------ .../mpu_iio/build/android/inv_mpu_iio-shared | Bin 0 -> 26464 bytes .../simple_apps/mpu_iio/build/android/shared.mk | 109 ++ .../software/simple_apps/mpu_iio/build/filelist.mk | 12 + .../software/simple_apps/mpu_iio/iio_utils.h | 643 ++++++++ .../software/simple_apps/mpu_iio/mpu_iio.c | 582 +++++++ .../self_test/build/android/inv_self_test-shared | Bin 11688 -> 16244 bytes .../software/simple_apps/self_test/inv_self_test.c | 338 ++-- 84 files changed, 3334 insertions(+), 8391 deletions(-) delete mode 100644 libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h delete mode 100644 libsensors_iio/software/core/HAL/include/mlos.h delete mode 100644 libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c delete mode 100644 libsensors_iio/software/core/HAL/linux/mlos_linux.c delete mode 100644 libsensors_iio/software/core/mllite/CMakeLists.txt delete mode 100644 libsensors_iio/software/core/mllite/Engineering.cmake create mode 100644 libsensors_iio/software/core/mllite/build/android/libmllite.so delete mode 100644 libsensors_iio/software/core/mllite/build/android/static.mk delete mode 100644 libsensors_iio/software/core/mllite/dmpconfig.txt delete mode 100644 libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c delete mode 100644 libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h delete mode 100644 libsensors_iio/software/core/mllite/mlmath.c delete mode 100644 libsensors_iio/software/core/mpl/adv_func.h delete mode 100644 libsensors_iio/software/core/mpl/auto_calibration.h delete mode 100644 libsensors_iio/software/core/mpl/build/android/static.mk delete mode 100644 libsensors_iio/software/core/mpl/interpolator.h delete mode 100644 libsensors_iio/software/core/mpl/inv_log.h delete mode 100644 libsensors_iio/software/core/mpl/mlsetinterrupts.h delete mode 100644 libsensors_iio/software/core/mpl/mlsupervisor_9axis.h delete mode 100644 libsensors_iio/software/core/mpl/orientation.h delete mode 100644 libsensors_iio/software/core/mpl/progressive_no_motion.h delete mode 100644 libsensors_iio/software/core/mpl/sensor_moments.h create mode 100644 libsensors_iio/software/core/mpl/shake.h delete mode 100644 libsensors_iio/software/core/mpl/state_storage.h delete mode 100644 libsensors_iio/software/simple_apps/common/external_hardware.h delete mode 100644 libsensors_iio/software/simple_apps/common/fopenCMake.c delete mode 100644 libsensors_iio/software/simple_apps/common/fopenCMake.h delete mode 100644 libsensors_iio/software/simple_apps/common/gestureMenu.c delete mode 100644 libsensors_iio/software/simple_apps/common/gestureMenu.h delete mode 100644 libsensors_iio/software/simple_apps/common/helper.c delete mode 100644 libsensors_iio/software/simple_apps/common/helper.h delete mode 100644 libsensors_iio/software/simple_apps/common/mlerrorcode.c delete mode 100644 libsensors_iio/software/simple_apps/common/mlerrorcode.h delete mode 100644 libsensors_iio/software/simple_apps/common/mlsetup.c delete mode 100644 libsensors_iio/software/simple_apps/common/mlsetup.h delete mode 100644 libsensors_iio/software/simple_apps/common/slave.h delete mode 100644 libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared delete mode 100644 libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk delete mode 100644 libsensors_iio/software/simple_apps/console/linux/build/filelist.mk delete mode 100644 libsensors_iio/software/simple_apps/console/linux/console_test.c delete mode 100644 libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared delete mode 100644 libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk delete mode 100644 libsensors_iio/software/simple_apps/input_sub/build/filelist.mk delete mode 100644 libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c create mode 100644 libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared create mode 100644 libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk create mode 100644 libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk create mode 100644 libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h create mode 100644 libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c diff --git a/libsensors_iio/CompassSensor.IIO.9150.cpp b/libsensors_iio/CompassSensor.IIO.9150.cpp index d9f2e0c..7d99af9 100644 --- a/libsensors_iio/CompassSensor.IIO.9150.cpp +++ b/libsensors_iio/CompassSensor.IIO.9150.cpp @@ -36,7 +36,7 @@ #define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) #if defined COMPASS_YAS530 -# warning "Invensense compass cal with YAS530 on primary bus" +# warning "Invensense compass cal with YAS530 IIO on primary bus" # define USE_MPL_COMPASS_HAL (1) # define COMPASS_NAME "INV_YAS530" #elif defined COMPASS_AK8975 @@ -44,7 +44,7 @@ # define USE_MPL_COMPASS_HAL (1) # define COMPASS_NAME "INV_AK8975" #elif defined INVENSENSE_COMPASS_CAL -# warning "Invensense compass cal with compass on secondary bus" +# warning "Invensense compass cal with compass IIO on secondary bus" # define USE_MPL_COMPASS_HAL (1) # define COMPASS_NAME "INV_COMPASS" #else @@ -59,9 +59,9 @@ CompassSensor::CompassSensor() : SensorBase(NULL, NULL), + compass_fd(-1), mCompassTimestamp(0), - mCompassInputReader(8), - compass_fd(-1) + mCompassInputReader(8) { VFUNC_LOG; @@ -107,7 +107,9 @@ CompassSensor::CompassSensor() LOGE("HAL:Couldn't read compass mounting matrix"); } - enable(ID_M, 0); + if (!isIntegrated()) { + enable(ID_M, 0); + } } CompassSensor::~CompassSensor() @@ -139,58 +141,25 @@ int CompassSensor::enable(int32_t handle, int en) VFUNC_LOG; mEnable = en; - int tempFd; int res; LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, compassSysFs.compass_enable, getTimestamp()); - tempFd = open(compassSysFs.compass_enable, O_RDWR); - res = errno; - if(tempFd < 0) { - LOGE("HAL:open of %s failed with '%s' (%d)", - compassSysFs.compass_enable, strerror(res), res); - return res; - } - res = enable_sysfs_sensor(tempFd, en); + res = write_sysfs_int(compassSysFs.compass_enable, en); LOGE_IF(res < 0, "HAL:enable compass failed"); - close(tempFd); if (en) { LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, compassSysFs.compass_x_fifo_enable, getTimestamp()); - tempFd = open(compassSysFs.compass_x_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - close(tempFd); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - compassSysFs.compass_x_fifo_enable, strerror(res), res); - } + res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en); LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, compassSysFs.compass_y_fifo_enable, getTimestamp()); - tempFd = open(compassSysFs.compass_y_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - close(tempFd); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - compassSysFs.compass_y_fifo_enable, strerror(res), res); - } + res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en); LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, compassSysFs.compass_z_fifo_enable, getTimestamp()); - tempFd = open(compassSysFs.compass_z_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - close(tempFd); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - compassSysFs.compass_z_fifo_enable, strerror(res), res); - } + res = write_sysfs_int(compassSysFs.compass_z_fifo_enable, en); } return res; @@ -327,20 +296,29 @@ void CompassSensor::fillList(struct sensor_t *list) if (compass) { if(!strcmp(compass, "INV_COMPASS")) { list->maxRange = COMPASS_MPU9150_RANGE; - list->resolution = COMPASS_MPU9150_RESOLUTION; + /* since target platform would use AK8963 instead of AK8975, + need to adopt AK8963's resolution here */ + // list->resolution = COMPASS_MPU9150_RESOLUTION; + list->resolution = COMPASS_AKM8963_RESOLUTION; list->power = COMPASS_MPU9150_POWER; list->minDelay = COMPASS_MPU9150_MINDELAY; return; } if(!strcmp(compass, "compass") - || !strcmp(compass, "INV_AK8975") - || !strcmp(compass, "INV_YAS530")) { + || !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_YAS530_RANGE; + list->resolution = COMPASS_YAS530_RESOLUTION; + list->power = COMPASS_YAS530_POWER; + list->minDelay = COMPASS_YAS530_MINDELAY; + return; + } if(!strcmp(compass, "INV_AMI306")) { list->maxRange = COMPASS_AMI306_RANGE; list->resolution = COMPASS_AMI306_RESOLUTION; @@ -390,16 +368,13 @@ int CompassSensor::inv_init_sysfs_attributes(void) inv_get_iio_trigger_path(iio_trigger_path); -#if defined COMPASS_YAS530 || defined COMPASS_AK8975 +#if defined COMPASS_AK8975 inv_get_input_number(COMPASS_NAME, &num); tbuf[0] = num + 0x30; tbuf[1] = 0; sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf); -#if defined COMPASS_YAS530 - strcat(sysfs_path, "/yas530"); -#else strcat(sysfs_path, "/ak8975"); -#endif + 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"); diff --git a/libsensors_iio/InputEventReader.h b/libsensors_iio/InputEventReader.h index 11c4e73..f0ccc63 100644 --- a/libsensors_iio/InputEventReader.h +++ b/libsensors_iio/InputEventReader.h @@ -1,17 +1,17 @@ -/* -* 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. +/* +* 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 diff --git a/libsensors_iio/MPLSensor.cpp b/libsensors_iio/MPLSensor.cpp index e23ecc9..90051cd 100644 --- a/libsensors_iio/MPLSensor.cpp +++ b/libsensors_iio/MPLSensor.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include "MPLSensor.h" #include "MPLSupport.h" @@ -46,7 +47,6 @@ #include "ml_sysfs_helper.h" // #define TESTING -// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */ #ifdef THIRD_PARTY_ACCEL # warning "Third party accel" @@ -80,11 +80,18 @@ static int hertz_request = 200; #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, {}}, + {"MPL Raw Gyro", "Invensense", 1, + SENSORS_RAW_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}}, {"MPL Accelerometer", "Invensense", 1, SENSORS_ACCELERATION_HANDLE, SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}}, @@ -103,6 +110,12 @@ static struct sensor_t sSensorList[] = {"MPL Gravity", "Invensense", 1, SENSORS_GRAVITY_HANDLE, SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}}, + +#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, {}}, +#endif }; MPLSensor *MPLSensor::gMPLSensor = NULL; @@ -134,17 +147,20 @@ static FILE *logfile = NULL; * MPLSensor class implementation ******************************************************************************/ +// following extended initializer list would only be available with -std=c++11 or -std=gnu+11 MPLSensor::MPLSensor(CompassSensor *compass) : SensorBase(NULL, NULL), mMplSensorInitialized(false), mNewData(0), mMasterSensorMask(INV_ALL_SENSORS), - mLocalSensorMask(ALL_MPL_SENSORS_NP), + mLocalSensorMask(0), mPollTime(-1), mHaveGoodMpuCal(0), mGyroAccuracy(0), mAccelAccuracy(0), mSampleCount(0), + dmp_orient_fd(-1), + mDmpOrientationEnabled(0), mEnabled(0), mOldEnabledMask(0), mAccelInputReader(4), @@ -155,8 +171,6 @@ MPLSensor::MPLSensor(CompassSensor *compass) mAccelScale(2), mPendingMask(0), mSensorMask(0), - mGyroOrientation{0}, - mAccelOrientation{0}, mFeatureActiveMask(0) { VFUNC_LOG; @@ -175,6 +189,8 @@ MPLSensor::MPLSensor(CompassSensor *compass) pthread_mutex_init(&mMplMutex, NULL); pthread_mutex_init(&mHALMutex, NULL); + bzero(mGyroOrientation, sizeof(mGyroOrientation)); + bzero(mAccelOrientation, sizeof(mAccelOrientation)); #ifdef INV_PLAYBACK_DBG LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); @@ -204,11 +220,10 @@ MPLSensor::MPLSensor(CompassSensor *compass) /* reset driver master enable */ masterEnable(0); - /* Load DMP image if capable, ie. MPU6xxx/9xxx */ - // TODO: disabled for GED tablet -#ifdef ENABLE_LP_QUAT_FEAT - loadDMP(); -#endif + 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); @@ -269,6 +284,11 @@ MPLSensor::MPLSensor(CompassSensor *compass) mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; + mPendingEvents[RawGyro].version = sizeof(sensors_event_t); + mPendingEvents[RawGyro].sensor = ID_RG; + mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE; + mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; + mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); mPendingEvents[Accelerometer].sensor = ID_A; mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; @@ -292,6 +312,7 @@ MPLSensor::MPLSensor(CompassSensor *compass) 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; @@ -306,7 +327,7 @@ MPLSensor::MPLSensor(CompassSensor *compass) /* setup MPL */ inv_constructor_init(); - /* load calibration file from /data/cal.bin */ + /* 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"); @@ -320,8 +341,16 @@ MPLSensor::MPLSensor(CompassSensor *compass) enableGyro(0); enableAccel(0); enableCompass(0); + + if (isLowPowerQuatEnabled()) { + enableLPQuaternion(0); + } + onPower(0); + enableDmpOrientation(isDmpDisplayOrientationOn()); + enableDmpOrientation(!isDmpScreenAutoRotationOn() && isDmpDisplayOrientationOn()); + #ifdef INV_PLAYBACK_DBG logfile = fopen("/data/playback.bin", "wb"); if (logfile) @@ -331,56 +360,61 @@ MPLSensor::MPLSensor(CompassSensor *compass) mMplSensorInitialized = true; } - void MPLSensor::enable_iio_sysfs() { VFUNC_LOG; - int tempFd = 0; char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN]; FILE *tempFp = NULL; - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:echo 1 > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", mpu.in_timestamp_en, getTimestamp()); - tempFd = open(mpu.in_timestamp_en, O_RDWR); - if(tempFd < 0) { + // Using fopen() here to benefit from fprintf() + tempFp = fopen(mpu.in_timestamp_en, "w"); + if (tempFp == NULL) { LOGE("HAL:could not open timestamp enable"); - } else if(enable_sysfs_sensor(tempFd, 1) < 0) { - LOGE("HAL:could not enable timestamp"); + } else { + if(fprintf(tempFp, "%d", 1) < 0) { + LOGE("HAL:could not enable timestamp"); + } + fclose(tempFp); } - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", + 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"); + } else { + if (fscanf(tempFp, "%s", iio_trigger_name) < 0) { + LOGE("HAL:could not read trigger name"); + } + fclose(tempFp); } - fclose(tempFp); - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:echo %s > %s (%lld)", + 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) { - LOGE("HAL:could not write current trigger"); + } else { + if (fprintf(tempFp, "%s", iio_trigger_name) < 0) { + LOGE("HAL:could not write current trigger"); + } + fclose(tempFp); } - fclose(tempFp); - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp()); tempFp = fopen(mpu.buffer_length, "w"); if (tempFp == NULL) { LOGE("HAL:could not open buffer length"); - } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) { - LOGE("HAL:could not write buffer length"); + } else { + if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) { + LOGE("HAL:could not write buffer length"); + } + fclose(tempFp); } - fclose(tempFp); inv_get_iio_device_node(iio_device_node); iio_fd = open(iio_device_node, O_RDONLY); @@ -428,7 +462,6 @@ int MPLSensor::inv_constructor_default_enable() return result; } - // TODO: double-check for GED tablet // result = inv_enable_motion_no_motion(); result = inv_enable_fast_nomot(); if (result) { @@ -452,8 +485,13 @@ int MPLSensor::inv_constructor_default_enable() 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); @@ -477,6 +515,9 @@ int MPLSensor::inv_constructor_default_enable() 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(); @@ -485,7 +526,6 @@ int MPLSensor::inv_constructor_default_enable() return result; } - // TODO: double-check for GED tablet result = inv_enable_quat_accuracy_monitor(); if (result) { LOG_RESULT_LOCATION(result); @@ -521,7 +561,7 @@ void MPLSensor::inv_set_device_properties() /* compass setup */ signed char orientMtx[9]; mCompassSensor->getOrientationMatrix(orientMtx); - orient = + orient = inv_orientation_matrix_to_scalar(orientMtx); long sensitivity; sensitivity = mCompassSensor->getSensitivity(); @@ -539,7 +579,7 @@ void MPLSensor::loadDMP() } /* load DMP firmware */ - LOGV_IF(SYSFS_VERBOSE, + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp()); fd = open(mpu.firmware_loaded, O_RDONLY); if(fd < 0) { @@ -564,7 +604,7 @@ void MPLSensor::loadDMP() } } - // onDMP(1); //Can't enable here. See note onDMP() + // onDMP(1); //Can't enable here. See note onDMP() } void MPLSensor::inv_get_sensors_orientation() @@ -572,13 +612,13 @@ void MPLSensor::inv_get_sensors_orientation() FILE *fptr; // get gyro orientation - LOGV_IF(SYSFS_VERBOSE, + 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[0], &om[1], &om[2], &om[3], &om[4], &om[5], &om[6], &om[7], &om[8]); fclose(fptr); @@ -601,13 +641,13 @@ void MPLSensor::inv_get_sensors_orientation() } // get accel orientation - LOGV_IF(SYSFS_VERBOSE, + 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], + 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); @@ -634,10 +674,11 @@ 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) @@ -645,18 +686,17 @@ MPLSensor::~MPLSensor() 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 */ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp()); - int fd = open(mpu.chip_enable, O_RDWR); - if(fd < 0) { - LOGE("HAL:could not open gyro chip enable"); - } else { - if(enable_sysfs_sensor(fd, 0) < 0) { - LOGE("HAL:could not disable gyro master enable"); - } + if(write_sysfs_int(mpu.chip_enable, 0) < 0) { + LOGE("HAL:could not disable gyro master enable"); } #ifdef INV_PLAYBACK_DBG @@ -665,18 +705,13 @@ MPLSensor::~MPLSensor() #endif } -#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) // ID_GY = 0 -#define A_ENABLED ((1 << ID_A) & enabled_sensors) // ID_A = 1 -#ifdef INVENSENSE_COMPASS_CAL // ID_M = 2 -#define M_ENABLED ((1 << ID_M) & enabled_sensors) -#else -// TODO: ID_M = 2 even for 3rd-party solution +#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) -#endif -#define O_ENABLED ((1 << ID_O) & enabled_sensors) // ID_O = 3 -#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4 -#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5 -#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6 +#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() @@ -685,7 +720,7 @@ int MPLSensor::setGyroInitialState() int res = 0; - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 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; @@ -733,38 +768,26 @@ int MPLSensor::onPower(int en) { VFUNC_LOG; - int res = 0; - char buf[sizeof(int)+1]; + int res; + int count, curr_power_state; - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.power_state, getTimestamp()); - int tempFd = open(mpu.power_state, O_RDWR); - res = errno; - if(tempFd < 0){ - LOGE("HAL:Open of %s failed with '%s' (%d)", - mpu.power_state, strerror(res), res); - } else { - // check and set new power state - count = read_attribute_sensor(tempFd, buf, sizeof(buf)); - if(count < 1) { - LOGE("HAL:Error reading power state"); - // will set power_state anyway - curr_power_state= -1; - } else { - sscanf(buf, "%d", &curr_power_state); - } - - if (en!=curr_power_state) { - if((res=enable_sysfs_sensor(tempFd, en)) < 0) { + 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); - close(tempFd); } + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:Power state already enable/disable curr=%d new=%d", + curr_power_state, en); } return res; } @@ -773,7 +796,7 @@ int MPLSensor::onDMP(int en) { VFUNC_LOG; - int res= -1; + int res = -1; int status; //Sequence to enable DMP @@ -782,24 +805,32 @@ int MPLSensor::onDMP(int en) //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()); if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){ LOGE("HAL:ERR can't get firmware_loaded status"); } else if (status == 1) { //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) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.dmp_on, getTimestamp()); if (write_sysfs_int(mpu.dmp_on, en) < 0) { - LOGE("HAL:ERR can't write dmp_on"); + LOGE("HAL:ERR can't write dmp_on"); } else { - res= 0; //Indicate write successful + res = 0; //Indicate write successful } //Enable DMP interrupt + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.dmp_int_on, getTimestamp()); if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { - LOGE("HAL:ERR can't en/dis DMP interrupt"); + LOGE("HAL:ERR can't en/dis DMP interrupt"); } } else { - res= 0; //DMP already set as requested + res = 0; //DMP already set as requested } } else { LOGE("HAL:ERR No DMP image"); @@ -824,7 +855,7 @@ int MPLSensor::enableLPQuaternion(int en) mFeatureActiveMask &= ~INV_DMP_QUATERNION; LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled"); } else { - if (enableQuaternionData(1) < 0 || onDMP(1) < 0) { + if (enableQuaternionData(1) < 0 || onDMP(1) < 0) { LOGE("HAL:ERR can't enable LP Quaternion"); } else { mFeatureActiveMask |= INV_DMP_QUATERNION; @@ -836,36 +867,52 @@ int MPLSensor::enableLPQuaternion(int en) int MPLSensor::enableQuaternionData(int en) { - int res= 0; + int res = 0; VFUNC_LOG; - //Enable DMP quaternion + // Enable DMP quaternion + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.quaternion_on, getTimestamp()); if (write_sysfs_int(mpu.quaternion_on, en) < 0) { LOGE("HAL:ERR can't write DMP quaternion_on"); - res= -1; //Indicate an err - } + res = -1; //Indicate an err + } if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off"); - inv_quaternion_sensor_was_turned_off(); + LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems"); } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems"); - if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) { - LOGE("HAL:ERR write in_quat_r_en"); - } - if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) { - LOGE("HAL:ERR write in_quat_x_en"); - } - if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) { - LOGE("HAL:ERR write in_quat_y_en"); - } - if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) { - LOGE("HAL:ERR write in_quat_z_en"); - } } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.in_quat_r_en, getTimestamp()); + if (write_sysfs_int(mpu.in_quat_r_en, en) < 0) { + LOGE("HAL:ERR write in_quat_r_en"); + } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.in_quat_x_en, getTimestamp()); + if (write_sysfs_int(mpu.in_quat_x_en, en) < 0) { + LOGE("HAL:ERR write in_quat_x_en"); + } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.in_quat_y_en, getTimestamp()); + if (write_sysfs_int(mpu.in_quat_y_en, en) < 0) { + LOGE("HAL:ERR write in_quat_y_en"); + } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.in_quat_z_en, getTimestamp()); - return res; + if (write_sysfs_int(mpu.in_quat_z_en, en) < 0) { + LOGE("HAL:ERR write in_quat_z_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) @@ -893,75 +940,38 @@ int MPLSensor::masterEnable(int en) { VFUNC_LOG; - int res = 0; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.chip_enable, getTimestamp()); - int tempFd = open(mpu.chip_enable, O_RDWR); - res = errno; - if(tempFd < 0){ - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.chip_enable, strerror(res), res); - return res; - } - res = enable_sysfs_sensor(tempFd, en); - return res; + return write_sysfs_int(mpu.chip_enable, en); } int MPLSensor::enableGyro(int en) { VFUNC_LOG; - int res = 0; + /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ + int res; /* need to also turn on/off the master enable */ - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.gyro_enable, getTimestamp()); - int tempFd = open(mpu.gyro_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.gyro_enable, strerror(res), res); - } + 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 { - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.gyro_x_fifo_enable, getTimestamp()); - tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.gyro_x_fifo_enable, strerror(res), res); - } + write_sysfs_int(mpu.gyro_x_fifo_enable, en); - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.gyro_y_fifo_enable, getTimestamp()); - tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.gyro_y_fifo_enable, strerror(res), res); - } + write_sysfs_int(mpu.gyro_y_fifo_enable, en); - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.gyro_z_fifo_enable, getTimestamp()); - tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.gyro_z_fifo_enable, strerror(res), res); - } + res = write_sysfs_int(mpu.gyro_z_fifo_enable, en); } return res; @@ -971,61 +981,31 @@ 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 */ - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - en, mpu.accel_enable, getTimestamp()); - int tempFd = open(mpu.accel_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.accel_enable, strerror(res), res); - } + /* need to also turn on/off the master enable */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.accel_enable, getTimestamp()); + 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 { - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.accel_x_fifo_enable, getTimestamp()); - tempFd = open(mpu.accel_x_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.accel_x_fifo_enable, strerror(res), res); - } + write_sysfs_int(mpu.accel_x_fifo_enable, en); - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.accel_y_fifo_enable, getTimestamp()); - tempFd = open(mpu.accel_y_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.accel_y_fifo_enable, strerror(res), res); - } + write_sysfs_int(mpu.accel_y_fifo_enable, en); - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", en, mpu.accel_z_fifo_enable, getTimestamp()); - tempFd = open(mpu.accel_z_fifo_enable, O_RDWR); - res = errno; - if (tempFd > 0) { - res = enable_sysfs_sensor(tempFd, en); - } else { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.accel_z_fifo_enable, strerror(res), res); - } + res = write_sysfs_int(mpu.accel_z_fifo_enable, en); } - if (!en && USE_THIRD_PARTY_ACCEL == 0) { - } - if(USE_THIRD_PARTY_ACCEL == 1 && en) { setAccelInitialState(); // BMA250 } @@ -1040,7 +1020,7 @@ int MPLSensor::enableCompass(int en) if (en == 0) { LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off"); inv_compass_was_turned_off(); - } + } return res; } @@ -1097,16 +1077,17 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { int off = 0; // Sequence to enable or disable a sensor - // 1. enable Power state + // 1. enable Power state // 2. reset master enable (=0) // 3. enable or disable a sensor // 4. set master enable (=1) - if (changed & ((1 << Gyro) | (1 << Accelerometer) | - (mCompassSensor->isIntegrated() << MagneticField))) { + if (isLowPowerQuatEnabled() || + changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | + (mCompassSensor->isIntegrated() << MagneticField))) { /* ensure power state is on */ onPower(1); - + /* reset master enable */ res = masterEnable(0); if(res < 0) { @@ -1116,14 +1097,14 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors); - if (changed & (1 << Gyro)) { - if(sensors & INV_THREE_AXIS_GYRO) { + if (changed & ((1 << Gyro) | (1 << RawGyro))) { + if (sensors & INV_THREE_AXIS_GYRO) { LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro"); res = enableGyro(on); if(res < 0) { return res; } - } else if((sensors & INV_THREE_AXIS_GYRO) == 0) { + } else if ((sensors & INV_THREE_AXIS_GYRO) == 0) { LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro"); res = enableGyro(off); if(res < 0) { @@ -1133,13 +1114,13 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { } if (changed & (1 << Accelerometer)) { - if(sensors & INV_THREE_AXIS_ACCEL) { + if (sensors & INV_THREE_AXIS_ACCEL) { LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel"); res = enableAccel(on); if(res < 0) { return res; } - } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) { + } else if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel"); res = enableAccel(off); if(res < 0) { @@ -1165,47 +1146,101 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) { } } -// TODO: disabled for GED tablet -#ifdef ENABLE_LP_QUAT_FEAT - // Enable LP Quat - if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity)))) { - if (!checkLPQuaternion()) { - enableLPQuaternion(1); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled"); + if ( isLowPowerQuatEnabled() ) { + // Enable LP Quat + if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity)))) { + if (!(changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | + (mCompassSensor->isIntegrated() << MagneticField)))) { + /* ensure power state is on */ + onPower(1); + /* reset master enable */ + res = masterEnable(0); + if(res < 0) { + return res; + } + } + if (!checkLPQuaternion()) { + enableLPQuaternion(1); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled"); + } + } else if (checkLPQuaternion()) { + enableLPQuaternion(0); } - } else if (checkLPQuaternion()) { - enableLPQuaternion(0); } -#endif - /* - if sensor & THREE_AXIS_GYRO - enable = 1 - if sensor & THREE_AXIS_ACCEL - enable = 1 - if compass_on_secondary - if sensor & THREE_AXIS_COMPASS - enable = 1 - else - enable = 0 - */ - if (changed & ((1 << Gyro) | (1 << Accelerometer) | + if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | (mCompassSensor->isIntegrated() << MagneticField))) { - if (sensors & - (INV_THREE_AXIS_GYRO - | INV_THREE_AXIS_ACCEL + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { + if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) { + // disable DMP event interrupt only (w/ data interrupt) + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { + res = -1; + LOGE("HAL:ERR can't disable DMP event interrupt"); + return res; + } + } + + if (isDmpDisplayOrientationOn()) { + // enable DMP + onDMP(1); + + res = enableAccel(on); + if(res < 0) { + return res; + } + + if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { + res = turnOffAccelFifo(); + } + if(res < 0) { + return res; + } + } + res = masterEnable(1); if(res < 0) { return res; } } else { // all sensors idle -> reduce power - res = onPower(0); - if(res < 0) { - return res; + if (isDmpDisplayOrientationOn()) { + // enable DMP + onDMP(1); + // enable DMP event interrupt only (no data interrupt) + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 1, mpu.dmp_event_int_on, getTimestamp()); + if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + res = enableAccel(on); + if(res < 0) { + return res; + } + if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { + res = turnOffAccelFifo(); + } + if(res < 0) { + return res; + } + res = masterEnable(1); + if(res < 0) { + return res; + } + } + else { + res = onPower(0); + if(res < 0) { + return res; + } } + storeCalibration(); } } @@ -1244,6 +1279,16 @@ int MPLSensor::gyroHandler(sensors_event_t* s) return update; } +int MPLSensor::rawGyroHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &s->gyro.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; @@ -1251,7 +1296,7 @@ int MPLSensor::accelHandler(sensors_event_t* s) update = inv_get_sensor_type_accelerometer( s->acceleration.v, &s->acceleration.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->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], s->timestamp, update); mAccelAccuracy = s->acceleration.status; return update; @@ -1317,9 +1362,17 @@ int MPLSensor::enable(int32_t handle, int en) VFUNC_LOG; android::String8 sname; - int what = -1; + 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()); + mDmpOrientationEnabled = !!en; + return 0; case ID_A: what = Accelerometer; sname = "Accelerometer"; @@ -1336,6 +1389,10 @@ int MPLSensor::enable(int32_t handle, int en) what = Gyro; sname = "Gyro"; break; + case ID_RG: + what = RawGyro; + sname = "RawGyro"; + break; case ID_GR: what = Gravity; sname = "Gravity"; @@ -1358,7 +1415,6 @@ int MPLSensor::enable(int32_t handle, int en) return -EINVAL; int newState = en ? 1 : 0; - int err = 0; unsigned long sen_mask; LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle, @@ -1367,7 +1423,6 @@ int MPLSensor::enable(int32_t handle, int en) LOGV_IF(PROCESS_VERBOSE, "HAL:%s sensor state change what=%d", sname.string(), what); - // TODO: disabled for GED tablet // pthread_mutex_lock(&mMplMutex); // pthread_mutex_lock(&mHALMutex); @@ -1389,6 +1444,7 @@ int MPLSensor::enable(int32_t handle, int en) switch (what) { case Gyro: + case RawGyro: case Accelerometer: case MagneticField: if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) | @@ -1422,7 +1478,7 @@ int MPLSensor::enable(int32_t handle, int en) // pthread_mutex_unlock(&mHALMutex); #ifdef INV_PLAYBACK_DBG - /* apparently the logging needs to be go through this sequence + /* apparently the logging needs to be go through this sequence to properly flush the log file */ inv_turn_off_data_logging(); fclose(logfile); @@ -1442,6 +1498,8 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) int what = -1; switch (handle) { + case ID_SO: + return 0; case ID_A: what = Accelerometer; sname = "Accelerometer"; @@ -1458,6 +1516,10 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) what = Gyro; sname = "Gyro"; break; + case ID_RG: + what = RawGyro; + sname = "RawGyro"; + break; case ID_GR: what = Gravity; sname = "Gravity"; @@ -1476,7 +1538,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) break; } -// TODO: disabled for GED tablet #if 0 // skip the 1st call for enalbing sensors called by ICS/JB sensor service static int counter_delay = 0; @@ -1500,7 +1561,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns); - // TODO: for GED tablet // limit all rates to reasonable ones */ /* if (ns < 10000000LL) { @@ -1516,6 +1576,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) switch (what) { case Gyro: + case RawGyro: case Accelerometer: for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); i++) { @@ -1529,6 +1590,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) 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; @@ -1539,6 +1601,11 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) 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); @@ -1548,7 +1615,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns) break; } - // TODO: disabled for GED tablet // pthread_mutex_lock(&mHALMutex); int res = update_delay(); // pthread_mutex_unlock(&mHALMutex); @@ -1562,33 +1628,31 @@ int MPLSensor::update_delay() { int64_t got; if (mEnabled) { - uint64_t wanted = -1LLU; - uint64_t wanted_ec = -1LLU; + int64_t wanted = 1000000000; + int64_t wanted_3rd_party_sensor = 1000000000; - // Sequence to change sensor's FIFO rate - // 1. enable Power state + // Sequence to change sensor's FIFO rate + // 1. enable Power state // 2. reset master enable // 3. Update delay // 4. set master enable - // TODO: unnecessary for IIO // ensure power on - // onPower(1); + onPower(1); - // TODO: unnecessary for IIO // reset master enable - // masterEnable(0); + masterEnable(0); /* search the minimum delay requested across all enabled sensors */ for (int i = 0; i < numSensors; i++) { if (mEnabled & (1 << i)) { - uint64_t ns = mDelays[i]; + int64_t ns = mDelays[i]; wanted = wanted < ns ? wanted : ns; } } - // same delay for ext compass - wanted_ec = wanted; + // 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 */ @@ -1607,25 +1671,6 @@ int MPLSensor::update_delay() { inv_set_accel_sample_rate(mplAccelRate); inv_set_compass_sample_rate(mplCompassRate); -// TODO: disabled for GED tablet -#ifdef ENABLE_LP_QUAT_FEAT - // Set LP Quaternion sample rate if enabled - if (checkLPQuaternion()) { - // TODO: need to verify this part for LPQ - if (wanted < 5000000LL) { - enableLPQuaternion(0); - } else { - inv_set_quat_sample_rate(rateInus); - // set DMP output rate to FIFO - write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted); - - //DMP running rate must be @ 200Hz - wanted= 5000000LL; - LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus); - } - } -#endif - /* TODO: Test 200Hz */ // inv_set_gyro_sample_rate(5000); LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate); @@ -1635,11 +1680,28 @@ int MPLSensor::update_delay() { int enabled_sensors = mEnabled; int tempFd = -1; if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { - uint64_t tempRate = wanted; + if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) { + 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, + 1000000000.f / tempRate, mpu.gyro_fifo_rate, getTimestamp()); tempFd = open(mpu.gyro_fifo_rate, O_RDWR); res = write_attribute_sensor(tempFd, 1000000000.f / tempRate); @@ -1650,15 +1712,16 @@ int MPLSensor::update_delay() { //nsToHz (BMA250) if(USE_THIRD_PARTY_ACCEL == 1) { LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", - wanted / 1000000L, mpu.accel_fifo_rate, + wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate, getTimestamp()); tempFd = open(mpu.accel_fifo_rate, O_RDWR); - res = write_attribute_sensor(tempFd, wanted / 1000000L); + res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L); LOGE_IF(res < 0, "HAL:ACCEL update delay error"); } if (!mCompassSensor->isIntegrated()) { - mCompassSensor->setDelay(ID_M, wanted_ec); + 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); } @@ -1679,7 +1742,14 @@ int MPLSensor::update_delay() { } else { if (GY_ENABLED) { - wanted = mDelays[Gyro]; + wanted = (mDelays[Gyro] <= mDelays[RawGyro]? + (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): + (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); + + if (isDmpDisplayOrientationOn()) { + 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); @@ -1690,9 +1760,18 @@ int MPLSensor::update_delay() { 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()) { + 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()); @@ -1711,12 +1790,20 @@ int MPLSensor::update_delay() { } else { if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) { wanted = mDelays[Gyro]; - } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) { + } + 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()) { + getDmpRate(&wanted); + } } + mCompassSensor->setDelay(ID_M, wanted); got = mCompassSensor->getDelay(ID_M); inv_set_compass_sample_rate(got / 1000); @@ -1724,37 +1811,20 @@ int MPLSensor::update_delay() { } - /* - if sensor & THREE_AXIS_GYRO - enable = 1 - if sensor & THREE_AXIS_ACCEL - enable = 1 - if compass_on_secondary - if sensor & THREE_AXIS_COMPASS - enable = 1 - else - enable = 0 - */ unsigned long sensors = mLocalSensorMask & mMasterSensorMask; - if (sensors & - (INV_THREE_AXIS_GYRO - | INV_THREE_AXIS_ACCEL + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { -// TODO: unnecessary for IIO -#if 0 res = masterEnable(1); if(res < 0) { return res; } -#endif } else { // all sensors idle -> reduce power -// TODO: unnecessary for IIO -#if 0 res = onPower(0); if(res < 0) { return res; } -#endif } } @@ -1762,6 +1832,7 @@ int MPLSensor::update_delay() { } /* use for third party accel */ +/* 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; @@ -1804,17 +1875,17 @@ int MPLSensor::readAccelEvents(sensors_event_t* data, int count) mAccelInputReader.next(); } - LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived); - return numEventReceived; } -/** +/** * Should be called after reading at least one of gyro - * compass or accel data. You should only read 1 sample of - * data and call this. + * 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; @@ -1858,36 +1929,40 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count) 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, nbyte; int i, nb, 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); + 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; -// TODO: disabled for GED tablet -#ifdef ENABLE_LP_QUAT_FEAT - lp_quaternion_on = checkLPQuaternion(); - if (lp_quaternion_on == 1) { - nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data + if (isLowPowerQuatEnabled()) { + lp_quaternion_on = checkLPQuaternion(); + if (lp_quaternion_on == 1) { + nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data + } } -#endif - // TODO: disabled for GED tablet // pthread_mutex_lock(&mMplMutex); // pthread_mutex_lock(&mHALMutex); - size_t rsize = read(iio_fd, rdata, nbyte); + ssize_t rsize = read(iio_fd, rdata, nbyte); if (sensors == 0) { // read(iio_fd, rdata, nbyte); - read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH); + rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); } -/* + +#ifdef TESTING LOGI("get one sample of IIO data with size: %d", rsize); LOGI("sensors: %d", sensors); @@ -1907,27 +1982,21 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { ((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 - // TODO: need to verify this for LPQ if (rsize < (nbyte - 8)) { - LOGE("HAL:ERR Full data packet was not read"); - // return -1; + LOGE("HAL:ERR Full data packet was not read. rsize=%ld nbyte=%d sensors=%d errno=%d(%s)", + rsize, nbyte, sensors, errno, strerror(errno)); + return -1; } -// TODO: disabled for GED tablet -#ifdef ENABLE_LP_QUAT_FEAT - if (lp_quaternion_on == 1) { + if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) { + for (i=0; i< 4; i++) { mCachedQuaternionData[i]= *(long*)rdata; rdata += sizeof(long); } } -/* - LOGV("HAL:rdata= %x sensors= %d lp_q_on= %d nbyte= %d rsize= %d", - rdata, sensors, lp_quaternion_on, nbyte, rsize); //tbd -*/ -#endif for (i = 0; i < 3; i++) { if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { @@ -1942,11 +2011,11 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { } } - mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0)); + 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 |= 4; + mask |= 1 << MagneticField; } mSensorTimestamp = *((long long *) (rdata + 8 * sensors)); @@ -1954,58 +2023,61 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { mCompassTimestamp = mSensorTimestamp; } - // send down temperature every 0.5 seconds - 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]); - } + 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); + 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 - } + } - if (mask & 1) { 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[0], mCachedGyroData[1], mCachedGyroData[2], mSensorTimestamp); } } - if (mask & 2) { + 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[0], mCachedAccelData[1], mCachedAccelData[2], mSensorTimestamp); } } - if ((mask & 4) && mCompassSensor->isIntegrated()) { + if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) { int status = 0; if (mCompassSensor->providesCalibration()) { status = mCompassSensor->getAccuracy(); @@ -2014,21 +2086,19 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) { 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], + LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], mCachedCompassData[2], mCompassTimestamp); } } -// TODO: disabled for GED tablet -#ifdef ENABLE_LP_QUAT_FEAT - if (lp_quaternion_on == 1) { + if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) { + 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 - %d", - mCachedQuaternionData[0], mCachedQuaternionData[1], + LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld", + mCachedQuaternionData[0], mCachedQuaternionData[1], mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp); } -#endif // pthread_mutex_unlock(&mMplMutex); // pthread_mutex_unlock(&mHALMutex); @@ -2048,12 +2118,16 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count) int done = 0; int nb; - // TODO: disabled for GED tablet - // TODO: for AMI306 // pthread_mutex_lock(&mMplMutex); // pthread_mutex_lock(&mHALMutex); done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); +#ifdef COMPASS_YAS530 + if (mCompassSensor->checkCoilsReset()==1) { + //Reset relevant compass settings + resetCompass(); + } +#endif if (done > 0) { int status = 0; if (mCompassSensor->providesCalibration()) { @@ -2063,8 +2137,8 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count) 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], + LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], mCachedCompassData[2], mCompassTimestamp); } } @@ -2075,6 +2149,27 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count) return numEventReceived; } +#ifdef COMPASS_YAS530 +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; @@ -2097,6 +2192,191 @@ int MPLSensor::getCompassFd() const return fd; } +int MPLSensor::turnOffAccelFifo() { + int i, res, tempFd; + 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++) { + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 0, accel_fifo_enable[i], getTimestamp()); + 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; + + // on power if not already On + onPower(1); + // reset master enable + masterEnable(0); + + if (en == 1) { + //Enable DMP orientation + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.display_orientation_on, getTimestamp()); + 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 + openDmpOrientFd(); + + //Enable DMP + onDMP(1); + + //Default DMP output rate to FIFO + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 5, mpu.dmp_output_rate, getTimestamp()); + if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) { + LOGE("HAL:ERR can't default DMP output rate"); + } + + //Set DMP rate to 200Hz + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + 200, mpu.accel_fifo_rate, getTimestamp()); + 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 + enableAccel(1); + + // disable accel FIFO + res = turnOffAccelFifo(); + + mFeatureActiveMask |=INV_DMP_DISPL_ORIENTATION; + } else { + // disable DMP + onDMP(0); + // disable accel engine + enableAccel(0); + } + + res = masterEnable(1); + 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 + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + int(1000000000.f / *wanted), mpu.dmp_output_rate, + getTimestamp()); + 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; @@ -2194,7 +2474,7 @@ int MPLSensor::inv_read_temperature(long long *data) return -1; } - LOGV_IF(ENG_VERBOSE, + LOGV_IF(ENG_VERBOSE, "HAL:temperature raw = %ld, timestamp = %lld, count = %d", raw, timestamp, count); data[0] = raw; @@ -2252,41 +2532,39 @@ int MPLSensor::inv_read_sensor_bias(int fd, long *data) 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)", + 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)", + 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)(7 * sizeof(sensor_t))) { + if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { LOGE("HAL:sensor list too small, not populating."); - return 0; + return -(sizeof(sSensorList) / sizeof(sensor_t)); } /* fill in the base values */ - memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); + memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t))); /* first add gyro, accel and compass to the list */ @@ -2301,7 +2579,7 @@ int MPLSensor::populateSensorList(struct sensor_t *list, int len) mCompassSensor->fillList(&list[MagneticField]); if(1) { - numsensors = 7; + numsensors = (sizeof(sSensorList) / sizeof(sensor_t)); /* all sensors will be added to the list fill in orientation values */ fillOrientation(list); @@ -2335,10 +2613,13 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; list[Accelerometer].power = ACCEL_MPU6050_POWER; - - // TODO: for GED tablet - // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY; - list[Accelerometer].minDelay = 5000; + 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) { @@ -2347,7 +2628,7 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) list[Accelerometer].power = ACCEL_MPU9150_POWER; list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY; return; - } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { + } 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; @@ -2378,10 +2659,12 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) list[Gyro].maxRange = GYRO_MPU6050_RANGE; list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; list[Gyro].power = GYRO_MPU6050_POWER; - - // TODO: for GED tablet - // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY; - list[Gyro].minDelay = 5000; + 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; @@ -2395,6 +2678,12 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 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; } @@ -2404,14 +2693,11 @@ void MPLSensor::fillRV(struct sensor_t *list) VFUNC_LOG; /* compute power on the fly */ - list[RotationVector].power = list[Gyro].power + + list[RotationVector].power = list[Gyro].power + list[Accelerometer].power + list[MagneticField].power; list[RotationVector].resolution = .00001; list[RotationVector].maxRange = 1.0; - - // TODO: for GED tablet - // list[RotationVector].minDelay = 10000; list[RotationVector].minDelay = 5000; return; @@ -2426,9 +2712,6 @@ void MPLSensor::fillOrientation(struct sensor_t *list) list[MagneticField].power; list[Orientation].resolution = .00001; list[Orientation].maxRange = 360.0; - - // TODO: for GED tablet - // list[Orientation].minDelay = 10000; list[Orientation].minDelay = 5000; return; @@ -2443,9 +2726,6 @@ void MPLSensor::fillGravity( struct sensor_t *list) list[MagneticField].power; list[Gravity].resolution = .00001; list[Gravity].maxRange = 9.81; - - // TODO: for GED tablet - // list[Gravity].minDelay = 10000; list[Gravity].minDelay = 5000; return; @@ -2460,9 +2740,6 @@ void MPLSensor::fillLinearAccel(struct sensor_t *list) list[MagneticField].power; list[LinearAccel].resolution = list[Accelerometer].resolution; list[LinearAccel].maxRange = list[Accelerometer].maxRange; - - // TODO: for GED tablet - // list[LinearAccel].minDelay = 10000; list[LinearAccel].minDelay = 5000; return; @@ -2484,7 +2761,7 @@ int MPLSensor::inv_init_sysfs_attributes(void) ALOGE("MPLSensor failed get sysfs path"); return -1; } - sysfs_names_ptr = + sysfs_names_ptr = (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); sptr = sysfs_names_ptr; if (sptr != NULL) { @@ -2512,6 +2789,7 @@ int MPLSensor::inv_init_sysfs_attributes(void) 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"); @@ -2528,18 +2806,15 @@ int MPLSensor::inv_init_sysfs_attributes(void) #ifdef THIRD_PARTY_ACCEL //BMA250 /* same as 'mpu.accel_enable' */ - sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/enable"); - sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate"); + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); + sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA"); sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA"); - sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/NA"); + sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); #else sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); - 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"); // TODO: for bias settings sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias"); @@ -2547,13 +2822,20 @@ int MPLSensor::inv_init_sysfs_attributes(void) sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); #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"); -#if 0 + 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++) { diff --git a/libsensors_iio/MPLSensor.h b/libsensors_iio/MPLSensor.h index 3ef1ba3..879f37b 100644 --- a/libsensors_iio/MPLSensor.h +++ b/libsensors_iio/MPLSensor.h @@ -28,17 +28,18 @@ #include "SensorBase.h" #include "InputEventReader.h" -// TODO: change to wanted vendor #ifdef INVENSENSE_COMPASS_CAL -#ifdef COMPASS_AMI306 +#ifdef COMPASS_YAS530 +#warning "unified HAL for YAS530" +#include "CompassSensor.IIO.primary.h" +#elif COMPASS_AMI306 #warning "unified HAL for AMI306" -#include "CompassSensor.IIO.AMI.h" +#include "CompassSensor.IIO.primary.h" #else #warning "unified HAL for MPU9150" #include "CompassSensor.IIO.9150.h" #endif - #else #warning "unified HAL for AKM" #include "CompassSensor.AKM.h" @@ -60,7 +61,6 @@ | INV_THREE_AXIS_COMPASS \ | INV_THREE_AXIS_GYRO) #else -// TODO: ID_M = 2 even for 3rd-party solution #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ | INV_THREE_AXIS_COMPASS \ | INV_THREE_AXIS_GYRO) @@ -70,6 +70,39 @@ #define INV_COMPASS_CAL 0x01 #define INV_COMPASS_FIT 0x02 #define INV_DMP_QUATERNION 0x04 +#define INV_DMP_DISPL_ORIENTATION 0x08 + +// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */ +// #define ENABLE_DMP_DISPL_ORIENT_FEAT /* Uncomment to enable DMP display orientation */ + +#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT +/* Uncomment followings to enable screen auto-rotation by DMP (need the latest Android source tree update) */ +// #define ENABLE_DMP_SCREEN_AUTO_ROTATION +#endif + +int isLowPowerQuatEnabled() { +#ifdef ENABLE_LP_QUAT_FEAT + return 1; +#else + return 0; +#endif +} + +int isDmpDisplayOrientationOn() { +#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT + return 1; +#else + return 0; +#endif +} + +int isDmpScreenAutoRotationOn() { +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + return 1; +#else + return 0; +#endif +} /*****************************************************************************/ /** MPLSensor implementation which fits into the HAL example for crespo provided @@ -82,12 +115,9 @@ class MPLSensor: public SensorBase typedef int (MPLSensor::*hfunc_t)(sensors_event_t*); public: - MPLSensor(CompassSensor *); - virtual ~MPLSensor(); - - enum - { + enum { Gyro = 0, + RawGyro, Accelerometer, MagneticField, Orientation, @@ -97,6 +127,9 @@ public: numSensors }; + MPLSensor(CompassSensor *); + virtual ~MPLSensor(); + virtual int setDelay(int32_t handle, int64_t ns); virtual int enable(int32_t handle, int enabled); int32_t getEnableMask() { return mEnabled; } @@ -124,6 +157,17 @@ public: 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: // Lets us know if the constructor was actually able to finish its job. // E.g. false if init sysfs failed. @@ -131,6 +175,7 @@ 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); @@ -156,7 +201,6 @@ protected: 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 update_delay_sysfs_sensor(int fd, uint64_t ns); 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); @@ -166,6 +210,9 @@ protected: int inv_read_sensor_bias(int fd, long *data); void inv_get_sensors_orientation(void); int inv_init_sysfs_attributes(void); +#ifdef COMPASS_YAS530 + int resetCompass(void); +#endif void setCompassDelay(int64_t ns); void enable_iio_sysfs(void); int enableTap(int); @@ -192,11 +239,14 @@ protected: 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]; - uint64_t mDelays[numSensors]; + int64_t mDelays[numSensors]; hfunc_t mHandlers[numSensors]; short mCachedGyroData[3]; long mCachedAccelData[3]; @@ -231,11 +281,12 @@ protected: 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 *dmp_output_rate; char *gyro_enable; char *gyro_fifo_rate; @@ -263,6 +314,9 @@ protected: char *trigger_name; char *current_trigger; char *buffer_length; + + char *display_orientation_on; + char *event_display_orientation; } mpu; char *sysfs_names_ptr; diff --git a/libsensors_iio/MPLSupport.cpp b/libsensors_iio/MPLSupport.cpp index a961d78..dcb12d2 100644 --- a/libsensors_iio/MPLSupport.cpp +++ b/libsensors_iio/MPLSupport.cpp @@ -1,3 +1,19 @@ +/* +* 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 #include #include @@ -61,29 +77,19 @@ int enable_sysfs_sensor(int fd, int en) { VFUNC_LOG; - int nb = -1; + int nb; int err = 0; - if (fd >= 0) { - char buf[2]; - if (en) { - buf[0] = '1'; - nb = write(fd, buf, 1); - } else { - buf[0] = '0'; - nb = write(fd, buf, 1); - } - buf[1] = '\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)", - buf[0], nb, strerror(err), err); - } - close(fd); - } else { - LOGV_IF(EXTRA_VERBOSE, "HAL:enable_sysfs_sensor - fd<0"); + 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; } @@ -117,12 +123,15 @@ int read_sysfs_int(char *filename, int *var) FILE *sysfsfp; sysfsfp = fopen(filename, "r"); - if (sysfsfp!=NULL) { - fscanf(sysfsfp, "%d\n", var); - fclose(sysfsfp); + 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 { - LOGE("HAL:ERR open file to read"); - res= -1; + res = -errno; + LOGE("HAL:ERR open file %s to read with error %d", filename, res); } return res; } @@ -135,10 +144,10 @@ int write_sysfs_int(char *filename, int var) sysfsfp = fopen(filename, "w"); if (sysfsfp!=NULL) { fprintf(sysfsfp, "%d\n", var); - fclose(sysfsfp); + fclose(sysfsfp); } else { - LOGE("HAL:ERR open file to write"); - res= -1; + res = -errno; + LOGE("HAL:ERR open file %s to read with error %d", filename, res); } return res; } diff --git a/libsensors_iio/libmllite.so b/libsensors_iio/libmllite.so index ed13b13..98147a2 100644 Binary files a/libsensors_iio/libmllite.so and b/libsensors_iio/libmllite.so differ diff --git a/libsensors_iio/libmplmpu.so b/libsensors_iio/libmplmpu.so index e2ab461..fbd346f 100644 Binary files a/libsensors_iio/libmplmpu.so and b/libsensors_iio/libmplmpu.so differ diff --git a/libsensors_iio/local_log_def.h b/libsensors_iio/local_log_def.h index b08fdf1..4127dd7 100644 --- a/libsensors_iio/local_log_def.h +++ b/libsensors_iio/local_log_def.h @@ -1,7 +1,24 @@ #ifndef LOCAL_LOG_DEF_H #define LOCAL_LOG_DEF_H -// -- workaround for different LOG definition in JellyBean -- +/* comment this line if Android OS is ICS and prior */ +#define ANDROID_VERSION_JB (1) + +/* 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 */ + +#ifdef ANDROID_VERSION_JB #define LOGV ALOGV #define LOGV_IF ALOGV_IF #define LOGD ALOGD @@ -20,21 +37,7 @@ #define LOG_ASSERT ALOG_ASSERT #define LOG ALOG #define IF_LOG IF_ALOG -// -- Workaround End -- - -/* 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 */ +#endif #define FUNC_LOG \ LOGV("%s", __PRETTY_FUNCTION__) diff --git a/libsensors_iio/sensor_params.h b/libsensors_iio/sensor_params.h index 88d5ba0..c51d87a 100644 --- a/libsensors_iio/sensor_params.h +++ b/libsensors_iio/sensor_params.h @@ -18,25 +18,37 @@ #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) +#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_AKM +//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) @@ -80,6 +92,16 @@ #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) @@ -139,22 +161,32 @@ //GYRO MPU3050 #define RAD_P_DEG (3.14159f / 180.f) #define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG) -#define GYRO_MPU3050_RESOLUTION (32.8f * 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 (16.4f * RAD_P_DEG) +#define GYRO_MPU6050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) #define GYRO_MPU6050_POWER (5.5f) #define GYRO_MPU6050_MINDELAY (1000) //GYRO MPU9150 #define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG) -#define GYRO_MPU9150_RESOLUTION (16.4f * 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 (16.4f * 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) diff --git a/libsensors_iio/sensors.h b/libsensors_iio/sensors.h index 0556c10..7264043 100644 --- a/libsensors_iio/sensors.h +++ b/libsensors_iio/sensors.h @@ -36,6 +36,20 @@ __BEGIN_DECLS #endif #define ID_MPL_BASE (0) + +enum { + ID_GY = ID_MPL_BASE, + ID_RG, + ID_A, + ID_M, + ID_O, + ID_RV, + ID_LA, + ID_GR, + ID_SO +}; + +/* #define ID_GY (ID_MPL_BASE) #define ID_A (ID_GY + 1) #define ID_M (ID_A + 1) @@ -43,15 +57,6 @@ __BEGIN_DECLS #define ID_RV (ID_O + 1) #define ID_LA (ID_RV + 1) #define ID_GR (ID_LA + 1) - -/*#define ID_MPL_BASE (0) -#define ID_RV (ID_MPL_BASE) -#define ID_LA (ID_RV + 1) -#define ID_GR (ID_LA + 1) -#define ID_GY (ID_GR + 1) -#define ID_A (ID_GY + 1) -#define ID_M (ID_A + 1) -#define ID_O (ID_M + 1) */ /*****************************************************************************/ diff --git a/libsensors_iio/sensors_mpl.cpp b/libsensors_iio/sensors_mpl.cpp index 990c5f5..4445309 100644 --- a/libsensors_iio/sensors_mpl.cpp +++ b/libsensors_iio/sensors_mpl.cpp @@ -36,10 +36,16 @@ /*****************************************************************************/ /* The SENSORS Module */ -#define LOCAL_SENSORS (7) -static struct sensor_t sSensorList[7]; -static int numSensors = LOCAL_SENSORS; +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION +#define LOCAL_SENSORS (numSensors + 1) +#else +#define LOCAL_SENSORS numSensors + +#endif + +static struct sensor_t sSensorList[LOCAL_SENSORS]; +static int sensors = LOCAL_SENSORS; static int open_sensors(const struct hw_module_t* module, const char* id, struct hw_device_t** device); @@ -48,7 +54,7 @@ static int sensors__get_sensors_list(struct sensors_module_t* module, struct sensor_t const** list) { *list = sSensorList; - return numSensors; + return sensors; } static struct hw_module_methods_t sensors_module_methods = { @@ -81,13 +87,13 @@ private: enum { mpl = 0, compass, + dmpOrient, numSensorDrivers, // wake pipe goes here numFds, }; struct pollfd mPollFds[numSensorDrivers]; SensorBase *mSensor; - CompassSensor *mCompassSensor; }; /******************************************************************************/ @@ -100,9 +106,9 @@ sensors_poll_context_t::sensors_poll_context_t() { // setup the callback object for handing mpl callbacks setCallbackObject(mplSensor); - + // populate the sensor list - numSensors = + sensors = mplSensor->populateSensorList(sSensorList, sizeof(sSensorList)); mSensor = mplSensor; @@ -113,12 +119,15 @@ sensors_poll_context_t::sensors_poll_context_t() { 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; - delete mCompassSensor; } int sensors_poll_context_t::activate(int handle, int enabled) { @@ -144,22 +153,16 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) if (nb > 0) { for (int i = 0; count && i < numSensorDrivers; i++) { - if (mPollFds[i].revents & POLLIN) { + if (mPollFds[i].revents & (POLLIN | POLLPRI)) { nb = 0; if (i == mpl) { nb = mSensor->readEvents(data, count); + mPollFds[i].revents = 0; } else if (i == compass) { nb = ((MPLSensor*) mSensor)->readCompassEvents(data, count); - } -/* - if (nb > 0) { - count -= nb; - nbEvents += nb; - data += nb; mPollFds[i].revents = 0; } - */ } } nb = ((MPLSensor*) mSensor)->executeOnData(data, count); @@ -167,8 +170,16 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) count -= nb; nbEvents += nb; data += nb; - mPollFds[mpl].revents = 0; - mPollFds[compass].revents = 0; + } + + if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) { + nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count); + mPollFds[dmpOrient].revents= 0; + if (isDmpScreenAutoRotationOn() && nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } } } diff --git a/libsensors_iio/software/build/android/shared.mk b/libsensors_iio/software/build/android/shared.mk index bc8548c..f3a123f 100644 --- a/libsensors_iio/software/build/android/shared.mk +++ b/libsensors_iio/software/build/android/shared.mk @@ -22,8 +22,7 @@ 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/console/linux/build/$(TARGET) -APP_FOLDERS += $(INV_ROOT)/simple_apps/input_sub/build/$(TARGET) +APP_FOLDERS = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET) APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET) INSTALL_DIR = $(CURDIR) diff --git a/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h b/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h deleted file mode 100644 index fce28ae..0000000 --- a/libsensors_iio/software/core/HAL/include/inv_sysfs_utils.h +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#ifndef _INV_SYSFS_UTILS_H_ -#define _INV_SYSFS_UTILS_H_ - -/** - * struct inv_sysfs_names_s - Files needed by user applications. - * @buffer: Ring buffer attached to FIFO. - * @enable: Turns on HW-to-ring buffer flow. - * @raw_data: Raw data from registers. - * @temperature: Temperature data from register. - * @fifo_rate: FIFO rate/ODR. - * @power_state: Power state (this is a five-star comment). - * @fsr: Full-scale range. - * @lpf: Digital low pass filter. - * @scale: LSBs / dps (or LSBs / Gs). - * @temp_scale: LSBs / degrees C. - * @temp_offset: Offset in LSBs. - */ -struct inv_sysfs_names_s { - - //Sysfs for ITG3500 & MPU6050 - const char *buffer; - const char *enable; - const char *raw_data; //Raw Gyro data - const char *temperature; - const char *fifo_rate; - const char *power_state; - const char *fsr; - const char *lpf; - const char *scale; //Gyro scale - const char *temp_scale; - const char *temp_offset; - const char *self_test; - //Starting Sysfs available for MPU6050 only - const char *accel_en; - const char *accel_fifo_en; - const char *accel_fs; - const char *clock_source; - const char *early_suspend_en; - const char *firmware_loaded; - const char *gyro_en; - const char *gyro_fifo_en; - const char *key; - const char *raw_accel; - const char *reg_dump; - const char *tap_on; - const char *dmp_firmware; -}; - -/* File IO. Typically won't be called directly by user application, but they'll - * be here for your enjoyment. - */ -int inv_sysfs_write(char *filename, long data); -int inv_sysfs_read(char *filename, long num_bytes, char *data); - -/* Helper APIs to extract specific data. */ -int inv_read_buffer(int fd, long *data, long long *timestamp); -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp); -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data); -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data); -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data); -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data); -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data); -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data); -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data); -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data); - -/* Scaled data. */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); - -#endif /* #ifndef _INV_SYSFS_UTILS_H_ */ - - diff --git a/libsensors_iio/software/core/HAL/include/mlos.h b/libsensors_iio/software/core/HAL/include/mlos.h deleted file mode 100644 index ce06b07..0000000 --- a/libsensors_iio/software/core/HAL/include/mlos.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef _MLOS_H -#define _MLOS_H - -#ifndef __KERNEL__ -#include -#endif - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#if defined(LINUX) || defined(__KERNEL__) -typedef unsigned int HANDLE; -#endif - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - - /* - MLOSCreateFile defines. - */ - -#define MLOS_GENERIC_READ ((unsigned int)0x80000000) -#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) -#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) -#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) -#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) - - /* ---------- */ - /* - Enums. - */ - /* ---------- */ - - /* --------------- */ - /* - Structures. - */ - /* --------------- */ - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ - -#ifndef __KERNEL__ -#include - 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); - - // Binary Semaphore - inv_error_t inv_create_binary_semaphore(HANDLE *semaphore); - inv_error_t inv_destroy_binary_semaphore(HANDLE handle); - inv_error_t inv_increment_binary_semaphore(HANDLE handle); - inv_error_t inv_decrement_binary_semaphore(HANDLE handle, long timeout_ms); - - inv_error_t inv_destroy_mutex(HANDLE handle); - - void inv_sleep(int mSecs); - inv_time_t 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 -#endif - -#ifdef __cplusplus -} -#endif -#endif /* _MLOS_H */ diff --git a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c deleted file mode 100644 index c45badd..0000000 --- a/libsensors_iio/software/core/HAL/linux/inv_sysfs_utils.c +++ /dev/null @@ -1,317 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "inv_sysfs_utils.h" - -/* General TODO list: - * Select more reasonable string lengths or use fseek and malloc. - */ - -/** - * inv_sysfs_write() - Write an integer to a file. - * @filename: Path to file. - * @data: Value to write to file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_write(char *filename, long data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; -} - -/** - * inv_sysfs_read() - Read a string from a file. - * @filename: Path to file. - * @num_bytes: Number of bytes to read. - * @data: Data from file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_read(char *filename, long num_bytes, char *data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "r"); - if (!fp) - return -errno; - count = fread(data, 1, num_bytes, fp); - fclose(fp); - return count; -} - -/** - * inv_read_buffer() - Read data from ring buffer. - * @fd: File descriptor for buffer file. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_buffer(int fd, long *data, long long *timestamp) -{ - char str[35]; - int count; - - count = read(fd, str, sizeof(str)); - if (!count) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_raw() - Read raw data. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - char str[40]; - int count; - - count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str); - if (count < 0) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_temperature_raw() - Read temperature. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp) -{ - char str[25]; - int count; - - count = inv_sysfs_read((char*)names->temperature, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd%lld", &data[0], timestamp); - if (count < 2) - return -EAGAIN; - return count; -} - -/** - * inv_read_fifo_rate() - Read fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data) -{ - char str[8]; - int count; - - count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_power_state() - Read power state. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data) -{ - char str[2]; - int count; - - count = inv_sysfs_read((char*)names->power_state, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", (short*)data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_scale() - Read scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data) -{ - char str[5]; - int count; - - count = inv_sysfs_read((char*)names->scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%f", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_scale() - Read temperature scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_offset() - Read temperature offset. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_q16() - Get data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count; - short raw[3]; - float scale; - count = inv_read_raw(names, (long*)raw, timestamp); - count += inv_read_scale(names, &scale); - data[0] = (long)(raw[0] * (65536.f / scale)); - data[1] = (long)(raw[1] * (65536.f / scale)); - data[2] = (long)(raw[2] * (65536.f / scale)); - return count; -} - -/** - * inv_read_q16() - Get temperature data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes read or a negative error code. - */ -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count = 0; - short raw; - static short scale, offset; - static unsigned char first_read = 1; - - if (first_read) { - count += inv_read_temp_scale(names, &scale); - count += inv_read_temp_offset(names, &offset); - first_read = 0; - } - count += inv_read_temperature_raw(names, &raw, timestamp); - data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f); - - return count; -} - -/** - * inv_write_fifo_rate() - Write fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data) -{ - return inv_sysfs_write((char*)names->fifo_rate, (long)data); -} - -/** - * inv_write_buffer_enable() - Enable/disable buffer in /dev. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->enable, (long)data); -} - -/** - * inv_write_power_state() - Turn device on/off. - * @names: Names of sysfs files. - * @data: 1 to turn on. - * Returns number of bytes written or a negative error code. - */ -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->power_state, (long)data); -} - - diff --git a/libsensors_iio/software/core/HAL/linux/mlos_linux.c b/libsensors_iio/software/core/HAL/linux/mlos_linux.c deleted file mode 100644 index 75f062e..0000000 --- a/libsensors_iio/software/core/HAL/linux/mlos_linux.c +++ /dev/null @@ -1,194 +0,0 @@ -/* - $License: - Copyright (C) 2011 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 -#include -#include -#include - -#include "stdint_invensense.h" - -#include "mlos.h" -#include - - -/* -------------- */ -/* - 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/libsensors_iio/software/core/driver/include/linux/mpu.h b/libsensors_iio/software/core/driver/include/linux/mpu.h index 9b29695..4391226 100644 --- a/libsensors_iio/software/core/driver/include/linux/mpu.h +++ b/libsensors_iio/software/core/driver/include/linux/mpu.h @@ -27,135 +27,8 @@ #ifdef __KERNEL__ #include #include -#elif defined LINUX -#include -#include -#else -#include "mltypes.h" #endif -struct mpu_read_write { - /* Memory address or register address depending on ioctl */ - __u16 address; - __u16 length; - __u8 *data; -}; - -enum mpuirq_data_type { - MPUIRQ_DATA_TYPE_MPU_DATA_READY_IRQ, - MPUIRQ_DATA_TYPE_MPU_FIFO_READY_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 - structure to report what and when - * @interruptcount : The number of times this IRQ has occured since open - * @irqtime : monotonic time of the IRQ in ns - * @data_type : The type of this IRQ enum mpuirq_data_type - * @data : Data associated with this IRQ - */ -struct mpuirq_data { - __u32 interruptcount; - __s64 irqtime_ns; - __u32 data_type; - __s32 data; -}; - -enum ext_slave_config_key { - /* TODO: Remove these first six. */ - MPU_SLAVE_CONFIG_ODR_SUSPEND, - MPU_SLAVE_CONFIG_ODR_RESUME, - MPU_SLAVE_CONFIG_FSR_SUSPEND, - MPU_SLAVE_CONFIG_FSR_RESUME, - MPU_SLAVE_CONFIG_IRQ_SUSPEND, - MPU_SLAVE_CONFIG_IRQ_RESUME, - MPU_SLAVE_CONFIG_ODR, - MPU_SLAVE_CONFIG_FSR, - MPU_SLAVE_CONFIG_MOT_THS, - MPU_SLAVE_CONFIG_NMOT_THS, - MPU_SLAVE_CONFIG_MOT_DUR, - MPU_SLAVE_CONFIG_NMOT_DUR, - MPU_SLAVE_CONFIG_IRQ, - MPU_SLAVE_WRITE_REGISTERS, - MPU_SLAVE_READ_REGISTERS, - MPU_SLAVE_CONFIG_INTERNAL_REFERENCE, - /* AMI 306 specific config keys */ - MPU_SLAVE_PARAM, - MPU_SLAVE_WINDOW, - MPU_SLAVE_READWINPARAMS, - MPU_SLAVE_SEARCHOFFSET, - /* MPU3050 and MPU6050 Keys */ - MPU_SLAVE_INT_CONFIG, - MPU_SLAVE_EXT_SYNC, - MPU_SLAVE_FULL_SCALE, - MPU_SLAVE_LPF, - MPU_SLAVE_CLK_SRC, - MPU_SLAVE_DIVIDER, - MPU_SLAVE_DMP_ENABLE, - MPU_SLAVE_FIFO_ENABLE, - MPU_SLAVE_DMP_CFG1, - MPU_SLAVE_DMP_CFG2, - MPU_SLAVE_TC, - MPU_SLAVE_GYRO, - MPU_SLAVE_ADDR, - MPU_SLAVE_PRODUCT_REVISION, - MPU_SLAVE_SILICON_REVISION, - MPU_SLAVE_PRODUCT_ID, - MPU_SLAVE_GYRO_SENS_TRIM, - MPU_SLAVE_ACCEL_SENS_TRIM, - MPU_SLAVE_RAM, - /* -------------------------- */ - 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_GYRO - * MPU_CONFIG_ACCEL - * MPU_CONFIG_COMPASS - * MPU_CONFIG_PRESSURE - * MPU_GET_CONFIG_GYRO - * 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 { - __u8 key; - __u16 len; - __u8 apply; - void *data; -}; - -enum ext_slave_type { - EXT_SLAVE_TYPE_GYROSCOPE, - EXT_SLAVE_TYPE_ACCEL, - EXT_SLAVE_TYPE_COMPASS, - EXT_SLAVE_TYPE_PRESSURE, - /*EXT_SLAVE_TYPE_TEMPERATURE */ - - EXT_SLAVE_NUM_TYPES -}; enum secondary_slave_type { SECONDARY_SLAVE_TYPE_NONE, SECONDARY_SLAVE_TYPE_ACCEL, @@ -204,127 +77,6 @@ enum ext_slave_id { }; #define INV_PROD_KEY(ver, rev) (ver * 100 + rev) - -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 - * - * @type: the type of slave device based on the enum ext_slave_type - * definitions. - * @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 { - __u8 type; - __u32 irq; - __u32 adapt_num; - __u32 bus; - __u8 address; - __s8 orientation[9]; - void *irq_data; - void *private_data; -}; - -struct fix_pnt_range { - __s32 mantissa; - __s32 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 { - __u8 reg; - __u8 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 - * @read_reg: starting register address to retrieve data. - * @read_len: length in bytes of the sensor data. Typically 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, - __u8 *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; - __u8 type; - __u8 id; - __u8 read_reg; - __u8 read_len; - __u8 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. @@ -334,6 +86,7 @@ struct ext_slave_descr { * @sec_slave_id: id of the secondary slave device * @secondary_i2c_address: secondary device's i2c address * @secondary_orientation: secondary device's orientation matrix + * @key: key for MPL library. * * Contains platform specific information on how to configure the MPU3050 to * work on this platform. The orientation matricies are 3x3 rotation matricies diff --git a/libsensors_iio/software/core/driver/include/log.h b/libsensors_iio/software/core/driver/include/log.h index 74c49f3..0a747ce 100644 --- a/libsensors_iio/software/core/driver/include/log.h +++ b/libsensors_iio/software/core/driver/include/log.h @@ -352,9 +352,9 @@ static inline void __print_result_location(int result, #define INV_ERROR_CHECK(r_1329) \ - if (r_1329) { \ - LOG_RESULT_LOCATION(r_1329); \ - return r_1329; \ + if (r_1329) { \ + LOG_RESULT_LOCATION(r_1329); \ + return r_1329; \ } #ifdef __cplusplus diff --git a/libsensors_iio/software/core/mllite/CMakeLists.txt b/libsensors_iio/software/core/mllite/CMakeLists.txt deleted file mode 100644 index 8650553..0000000 --- a/libsensors_iio/software/core/mllite/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -## CMakeLists for mlsdk/mldmp - -project(mldmp C) - -if (NOT CMAKE_BUILD_ENGINEERING) - -# # just copy the library from mldmp/mpl/$PLATFORM to mldmp/ -# if (CMAKE_SYSTEM_NAME MATCHES Windows) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/win32/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/mpl.lib -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Android) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/android/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Linux) -# message("copying ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a to ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a") -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/libmpl.a ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# elseif (CMAKE_SYSTEM_NAME MATCHES Catalina) -# execute_process( -# COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/mpl/linux/mpl.lib ${CMAKE_CURRENT_BINARY_DIR}/libmpl.a -# ) -# endif() -# # better way that doesn't work for now -# # add_custom_command( -# # TARGET mpl -# # PRE_BUILD -# # COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${DDF} ${CMAKE_CURRENT_BINARY_DIR} -# # COMMAND ${CMAKE_COMMAND} ARGS -E echo "copying ${DDF}" -# # ) - -else (NOT CMAKE_BUILD_ENGINEERING) - - set (CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) - include(Engineering) - -endif (NOT CMAKE_BUILD_ENGINEERING) diff --git a/libsensors_iio/software/core/mllite/Engineering.cmake b/libsensors_iio/software/core/mllite/Engineering.cmake deleted file mode 100644 index 42f2429..0000000 --- a/libsensors_iio/software/core/mllite/Engineering.cmake +++ /dev/null @@ -1,150 +0,0 @@ -## Engineering CMakeLists for software/core/mllite - -include_directories( - . - ${SOLUTION_SOURCE_DIR}/core/mpl - ${SOLUTION_SOURCE_DIR}/core/HAL - ${SOLUTION_SOURCE_DIR}/driver/include -) - -add_definitions ( - -DINV_INCLUDE_LEGACY_HEADERS -) - -if (CMAKE_SYSTEM_NAME MATCHES Android) - - include_directories( - ${SOLUTION_SOURCE_DIR}/core/mllite/linux - ${SOLUTION_SOURCE_DIR}/driver/include/linux - ) - add_definitions( - -DLINUX - -D_REENTRANT - ) - set (HEADERS - ${HEADERS} - linux/mlos.h - linux/ml_stored_data.h - linux/ml_load_dmp.h - linux/ml_sysfs_helper.h - ) - set (SOURCES - ${SOURCES} - linux/mlos_linux.c - linux/ml_stored_data.c - linux/ml_load_dmp.c - linux/ml_sysfs_helper.c - ) - -elseif (CMAKE_SYSTEM_NAME MATCHES Linux) - - add_definitions( - -DLINUX - -D_REENTRANT - ) - set (HEADERS - ${HEADERS} - ) - set (SOURCES - ${SOURCES} - ) - -elseif (CMAKE_SYSTEM_NAME MATCHES Windows) - - add_definitions( - -DAIO - -DUART_COM - -D_CRT_SECURE_NO_WARNINGS - -D_CRT_SECURE_NO_DEPRECATE - ) - set (HEADERS - ${HEADERS} - ) - set (SOURCES - ${SOURCES} - ) - -endif () - -set (HEADERS - ${HEADERS} - data_builder.h - hal_outputs.h - message_layer.h - ml_math_func.h - mpl.h - results_holder.h - start_manager.h - storage_manager.h -) -set (SOURCES - ${SOURCES} - data_builder.c - hal_outputs.c - message_layer.c - ml_math_func.c - mpl.c - results_holder.c - start_manager.c - storage_manager.c -) - -# coveniently add this file to the resources for easy access within IDEs -set (RESOURCES - Engineering.cmake -) - -if (CMAKE_TESTING_SUPPORT) - - message("Including Testing support") - include_directories( - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport - ) - add_definitions( - -DSELF_VERIFICATION - ) - set( - HEADERS - ${HEADERS} - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.h - ) - set( - SOURCES - ${SOURCES} - ${SOLUTION_SOURCE_DIR}/mltools/debugsupport/testsupport.c - ) - -endif () - -############ -## TARGET ## -############ -add_library( - mllite STATIC - ${SOURCES} - ${HEADERS} - ${RESOURCES} -) -set_target_properties( - mllite - PROPERTIES CLEAN_DIRECT_OUTPUT 1 -) - -if (CMAKE_SYSTEM_NAME MATCHES "(Android|Linux)") - - add_library( - mllite_shared SHARED - ${SOURCES} - ${HEADERS} - ${RESOURCES} - ) - FIX_SHARED_LIBRARY_NAME("mllite_shared" "mllite") - - install ( - TARGETS mllite_shared - DESTINATION lib - ) - -endif () - - diff --git a/libsensors_iio/software/core/mllite/build/android/libmllite.so b/libsensors_iio/software/core/mllite/build/android/libmllite.so new file mode 100644 index 0000000..98147a2 Binary files /dev/null and b/libsensors_iio/software/core/mllite/build/android/libmllite.so differ diff --git a/libsensors_iio/software/core/mllite/build/android/static.mk b/libsensors_iio/software/core/mllite/build/android/static.mk deleted file mode 100644 index 6ad45de..0000000 --- a/libsensors_iio/software/core/mllite/build/android/static.mk +++ /dev/null @@ -1,110 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -OBJFOLDER = $(CURDIR)/obj - -CROSS = arm-none-linux-gnueabi- -COMP= $(CROSS)gcc -LINK= $(CROSS)ar cr - -MLLITE_DIR = $(MLSDK_ROOT)/mllite -MPL_DIR = $(MLSDK_ROOT)/mldmp -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform - -include $(MLSDK_ROOT)/Android-common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += -Wall -fpic -CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -DLINUX -DANDROID -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(MLPLATFORM_DIR)/include -CFLAGS += -I$(MLSDK_ROOT)/mlutils -CFLAGS += -I$(MLSDK_ROOT)/mlapps/common -CFLAGS += $(MLSDK_INCLUDES) -CFLAGS += $(MLSDK_DEFINES) - -VPATH += $(MLLITE_DIR) -VPATH += $(MLSDK_ROOT)/mlutils -VPATH += $(MLLITE_DIR)/accel -VPATH += $(MLLITE_DIR)/compass -VPATH += $(MLLITE_DIR)/pressure -VPATH += $(MLLITE_DIR)/mlapps/common - -#################################################################################################### -## sources - -ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) - -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c -ML_SOURCES += $(MLLITE_DIR)/accel.c -ML_SOURCES += $(MLLITE_DIR)/compass.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c -ML_SOURCES += $(MLLITE_DIR)/key0_96.c -ML_SOURCES += $(MLLITE_DIR)/pressure.c -ML_SOURCES += $(MLLITE_DIR)/ml.c -ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c -ML_SOURCES += $(MLLITE_DIR)/ml_init.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c -ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c -ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c -ML_SOURCES += $(MLLITE_DIR)/mldl.c -ML_SOURCES += $(MLLITE_DIR)/mldmp.c -ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c -ML_SOURCES += $(MLLITE_DIR)/mlstates.c -ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c -ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c -ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c -ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c -ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c -ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c -ifeq ($(MPU_NAME),MPU3050) -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c -else -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c -endif - -ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) -ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all clean cleanall - -all: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n\n") - mkdir obj - -$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n\n") - $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c index f70be7c..b139771 100644 --- a/libsensors_iio/software/core/mllite/data_builder.c +++ b/libsensors_iio/software/core/mllite/data_builder.c @@ -23,6 +23,7 @@ #include "mlmath.h" #include "storage_manager.h" #include "message_layer.h" +#include "results_holder.h" #include "log.h" #undef MPL_LOG_TAG @@ -48,6 +49,10 @@ struct inv_db_save_t { /** 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 { @@ -69,7 +74,7 @@ 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 53394 +#define INV_DB_SAVE_KEY 53395 #ifdef INV_PLAYBACK_DBG @@ -98,6 +103,14 @@ void inv_turn_off_data_logging() 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; } @@ -162,119 +175,7 @@ void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, sensor->sensitivity = sensitivity; sensor->orientation = orientation; } -void inv_get_quaternion_transformation(int orientation, long *q) -{ - long s = 759250125; // sqrt(.5)*2^30 - - switch (orientation) - { - case 0x70: - q[0] = 759250125; - q[1] = 759250125; - q[2] = 0; - q[3] = 0; - break; - case 0x10a: - q[0] = 759250125; - q[1] = 0; - q[2] = 759250125; - q[3] = 0; - break; - case 0x85: - q[0] = 759250125; - q[1] = 0; - q[2] = 0; - q[3] = 759250125; - break; - case 0x181: - q[0] = 0; - q[1] = 759250125; - q[2] = 759250125; - q[3] = 0; - break; - case 0x2a: - q[0] = 0; - q[1] = 759250125; - q[2] = 0; - q[3] = 759250125; - break; - case 0x54: - q[0] = 0; - q[1] = 0; - q[2] = 759250125; - q[3] = 759250125; - break; - case 0x150: - q[0] = 759250125; - q[1] = -759250125; - q[2] = 0; - q[3] = 0; - break; - case 0xe: - q[0] = 759250125; - q[1] = 0; - q[2] = -759250125; - q[3] = 0; - break; - case 0xa1: - q[0] = 759250125; - q[1] = 0; - q[2] = 0; - q[3] = -759250125; - break; - case 0x1a5: - q[0] = 0; - q[1] = 759250125; - q[2] = -759250125; - q[3] = 0; - break; - case 0x12e: - q[0] = 0; - q[1] = 759250125; - q[2] = 0; - q[3] = -759250125; - break; - case 0x174: - q[0] = 0; - q[1] = 0; - q[2] = 759250125; - q[3] = -759250125; - break; - - - case 0x88: - q[0] = 1073741824; - q[1] = 0; - q[2] = 0; - q[3] = 0; - break; - - - case 0x1a8: - q[0] = 0; - q[1] = 1073741824; - q[2] = 0; - q[3] = 0; - break; - - case 0x18c: - q[0] = 0; - q[1] = 0; - q[2] = 1073741824; - q[3] = 0; - break; - - case 0xac: - q[0] = 0; - q[1] = 0; - q[2] = 0; - q[3] = 1073741824; - break; - - default: - break; - } -} + /** 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() @@ -354,6 +255,22 @@ void inv_set_compass_sample_rate(long sample_rate_us) 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 */ @@ -498,7 +415,7 @@ void inv_matrix_vector_mult(const long *A, const long *x, long *y) } /** Takes raw data stored in the sensor, removes bias, and converts it to -* calibrated data in the body frame. +* 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. @@ -508,15 +425,17 @@ 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] << 16; - raw32[1] = (long)sensor->raw[1] << 16; - raw32[2] = (long)sensor->raw[2] << 16; + raw32[0] = (long)sensor->raw[0] << 15; + raw32[1] = (long)sensor->raw[1] << 15; + raw32[2] = (long)sensor->raw[2] << 15; - raw32[0] -= bias[0]; - raw32[1] -= bias[1]; - raw32[2] -= bias[2]; + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_data); - inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated); + 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; } @@ -539,6 +458,7 @@ void inv_set_compass_bias(const long *bias, int accuracy) 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); } @@ -566,6 +486,7 @@ void inv_set_accel_bias(const long *bias, 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); } @@ -590,6 +511,7 @@ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); } sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; } @@ -607,6 +529,8 @@ void inv_set_gyro_bias(const long *bias, int accuracy) } } 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]; @@ -687,6 +611,7 @@ inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) 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; @@ -757,6 +682,7 @@ inv_error_t inv_build_compass(const long *compass, int status, 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; @@ -1077,6 +1003,22 @@ void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) } } +/** 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_data, sizeof(sensors.gyro.raw_data)); + 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. */ diff --git a/libsensors_iio/software/core/mllite/data_builder.h b/libsensors_iio/software/core/mllite/data_builder.h index b2d0881..4263922 100644 --- a/libsensors_iio/software/core/mllite/data_builder.h +++ b/libsensors_iio/software/core/mllite/data_builder.h @@ -56,6 +56,7 @@ extern "C" { #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 { @@ -69,6 +70,8 @@ struct inv_single_sensor_t { int orientation; /** The raw data in raw data units in the mounting frame */ short raw[3]; + /** Raw data in body frame */ + long raw_data[3]; /** Calibrated data */ long calibrated[3]; long sensitivity; @@ -160,6 +163,10 @@ 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); @@ -197,6 +204,7 @@ 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); diff --git a/libsensors_iio/software/core/mllite/dmpconfig.txt b/libsensors_iio/software/core/mllite/dmpconfig.txt deleted file mode 100644 index 4643ed5..0000000 --- a/libsensors_iio/software/core/mllite/dmpconfig.txt +++ /dev/null @@ -1,3 +0,0 @@ -major version = 1 -minor version = 0 -startAddr = 0 diff --git a/libsensors_iio/software/core/mllite/hal_outputs.c b/libsensors_iio/software/core/mllite/hal_outputs.c index 1cd3b81..5e7b2cc 100644 --- a/libsensors_iio/software/core/mllite/hal_outputs.c +++ b/libsensors_iio/software/core/mllite/hal_outputs.c @@ -1,425 +1,475 @@ -/* - $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 "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 */ - inv_time_t nav_timestamp; - inv_time_t gam_timestamp; - inv_time_t accel_timestamp; - long nav_quat[4]; - int gyro_status; - int accel_status; - int compass_status; - int nine_axis_status; -}; - -static struct hal_output_t hal_out; - -/** 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_get_accel_set(accel, accuracy, timestamp); - 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 hal_out.nine_axis_status; -} - -/** 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]; - int status; - - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - 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; - if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) - status = 1; - else - status = 0; - return status; -} - -/** Gyroscope 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) -{ - /* 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 - 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; -} - -/** -* 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}.
-* 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 = hal_out.accuracy_mag; - *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 - long compass[3]; - inv_get_compass_set(compass, accuracy, timestamp); - values[0] = compass[0] * COMPASS_CONVERSION; - values[1] = compass[1] * COMPASS_CONVERSION; - values[2] = compass[2] * COMPASS_CONVERSION; - if (hal_out.compass_status & INV_NEW_DATA) - status = 1; - else - status = 0; - return status; -} - - -/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. -* @param[out] values Length 3, Degrees.
-* - 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
-* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values -* when the z-axis moves toward the y-axis.
-* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive -* values when the x-axis moves toward the z-axis.
-* -* @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) -{ - long t1, t2, t3; - long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; - - *accuracy = hal_out.accuracy_mag; - *timestamp = hal_out.nav_timestamp; - - q00 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[0]); - q01 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[1]); - q02 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[2]); - q03 = inv_q29_mult(hal_out.nav_quat[0], hal_out.nav_quat[3]); - q11 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[1]); - q12 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[2]); - q13 = inv_q29_mult(hal_out.nav_quat[1], hal_out.nav_quat[3]); - q22 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[2]); - q23 = inv_q29_mult(hal_out.nav_quat[2], hal_out.nav_quat[3]); - q33 = inv_q29_mult(hal_out.nav_quat[3], hal_out.nav_quat[3]); - - /* X component of the Ybody axis in World frame */ - t1 = q12 - q03; - - /* Y component of the Ybody axis in World frame */ - t2 = q22 + q00 - (1L << 30); - values[0] = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; - if (values[0] < 0) - values[0] += 360; - - /* Z component of the Ybody axis in World frame */ - t3 = q23 + q01; - values[1] = - -atan2f((float) t3, - sqrtf((float) t1 * t1 + - (float) t2 * t2)) * 180.f / (float) M_PI; - /* Z component of the Zbody axis in World frame */ - t2 = q33 + q00 - (1L << 30); - if (t2 < 0) { - if (values[1] >= 0) - values[1] = 180.f - values[1]; - else - values[1] = -180.f - values[1]; - } - - /* X component of the Xbody axis in World frame */ - t1 = q11 + q00 - (1L << 30); - /* Y component of the Xbody axis in World frame */ - t2 = q12 + q03; - /* Z component of the Xbody axis in World frame */ - t3 = q13 - q02; - //values[2] = atan2f((float)t3,sqrtf((float)t1*t1+(float)t2*t2))*180.f/(float)M_PI; - - values[2] = - -(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) * - 180.f / (float) M_PI - 90); - if (values[2] >= 90) - values[2] = 180 - values[2]; - - if (values[2] < -90) - values[2] = -180 - values[2]; - - 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; - (void) sensor_cal; - inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_mag, - &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; - } - - switch (use_sensor) { - default: - 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; - } - - 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; -} - -/** 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) -{ - memset(&hal_out, 0, sizeof(hal_out)); - 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; -} - -/** - * @} - */ +/* + $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 "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]; +}; + +static struct hal_output_t hal_out; + +/** 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_get_accel_set(accel, accuracy, timestamp); + 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 hal_out.nine_axis_status; +} + +/** 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]; + int status; + + *accuracy = (int8_t) hal_out.accuracy_quat; + *timestamp = hal_out.nav_timestamp; + 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; + if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) + status = 1; + else + status = 0; + return status; +} + +/** 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) +{ + /* 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 + 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) +{ + /* 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 + 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}.
+* 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.
+* - 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
+* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values +* when the z-axis moves toward the y-axis.
+* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive +* values when the x-axis moves toward the z-axis.
+* +* @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; + } + + switch (use_sensor) { + default: + 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; + } + + /* 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/libsensors_iio/software/core/mllite/hal_outputs.h b/libsensors_iio/software/core/mllite/hal_outputs.h index ed4cb65..85e88f3 100644 --- a/libsensors_iio/software/core/mllite/hal_outputs.h +++ b/libsensors_iio/software/core/mllite/hal_outputs.h @@ -19,6 +19,8 @@ extern "C" { 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, diff --git a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c deleted file mode 100644 index 649b917..0000000 --- a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.c +++ /dev/null @@ -1,318 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "inv_sysfs_utils.h" - -/* General TODO list: - * Select more reasonable string lengths or use fseek and malloc. - */ - -/** - * inv_sysfs_write() - Write an integer to a file. - * @filename: Path to file. - * @data: Value to write to file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_write(char *filename, long data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; -} - -/** - * inv_sysfs_read() - Read a string from a file. - * @filename: Path to file. - * @num_bytes: Number of bytes to read. - * @data: Data from file. - * Returns number of bytes written or a negative error code. - */ -int inv_sysfs_read(char *filename, long num_bytes, char *data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "r"); - if (!fp) - return -errno; - count = fread(data, 1, num_bytes, fp); - fclose(fp); - return count; -} - -/** - * inv_read_buffer() - Read data from ring buffer. - * @fd: File descriptor for buffer file. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_buffer(int fd, long *data, long long *timestamp) -{ - char str[35]; - int count; - - count = read(fd, str, sizeof(str)); - if (!count) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_raw() - Read raw data. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. Use NULL if unsupported. - * Returns number of bytes written or a negative error code. - */ -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - char str[40]; - int count; - - count = inv_sysfs_read((char*)names->raw_data, sizeof(str), str); - if (count < 0) - return count; - if (!timestamp) - count = sscanf(str, "%ld%ld%ld", &data[0], &data[1], &data[2]); - else - count = sscanf(str, "%ld%ld%ld%lld", &data[0], &data[1], - &data[2], timestamp); - if (count < (timestamp?4:3)) - return -EAGAIN; - return count; -} - -/** - * inv_read_temperature_raw() - Read temperature. - * @names: Names of sysfs files. - * @data: Data in hardware units. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp) -{ - char str[25]; - int count; - - count = inv_sysfs_read((char*)names->temperature, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd%lld", &data[0], timestamp); - if (count < 2) - return -EAGAIN; - return count; -} - -/** - * inv_read_fifo_rate() - Read fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data) -{ - char str[8]; - int count; - - count = inv_sysfs_read((char*)names->fifo_rate, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_power_state() - Read power state. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data) -{ - char str[2]; - int count; - - count = inv_sysfs_read((char*)names->power_state, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", (short*)data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_scale() - Read scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data) -{ - char str[5]; - int count; - - count = inv_sysfs_read((char*)names->scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%f", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_scale() - Read temperature scale. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_scale, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_temp_offset() - Read temperature offset. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * Returns number of bytes written or a negative error code. - */ -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data) -{ - char str[4]; - int count; - - count = inv_sysfs_read((char*)names->temp_offset, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", data); - if (count < 1) - return -EAGAIN; - return count; -} - -/** - * inv_read_q16() - Get data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes written or a negative error code. - */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count; - short raw[3]; - float scale; - count = inv_read_raw(names, (long*)raw, timestamp); - count += inv_read_scale(names, &scale); - data[0] = (long)(raw[0] * (65536.f / scale)); - data[1] = (long)(raw[1] * (65536.f / scale)); - data[2] = (long)(raw[2] * (65536.f / scale)); - return count; -} - -/** - * inv_read_q16() - Get temperature data as q16 fixed point. - * @names: Names of sysfs files. - * @data: 1 if device is on. - * @timestamp: Time when data was read from device. - * Returns number of bytes read or a negative error code. - */ -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp) -{ - int count = 0; - short raw; - static short scale, offset; - static unsigned char first_read = 1; - - if (first_read) { - count += inv_read_temp_scale(names, &scale); - count += inv_read_temp_offset(names, &offset); - first_read = 0; - } - count += inv_read_temperature_raw(names, &raw, timestamp); - data[0] = (long)((35 + ((float)(raw - offset) / scale)) * 65536.f); - - return count; -} - -/** - * inv_write_fifo_rate() - Write fifo rate. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data) -{ - return inv_sysfs_write((char*)names->fifo_rate, (long)data); -} - -/** - * inv_write_buffer_enable() - Enable/disable buffer in /dev. - * @names: Names of sysfs files. - * @data: Fifo rate. - * Returns number of bytes written or a negative error code. - */ -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->enable, (long)data); -} - -/** - * inv_write_power_state() - Turn device on/off. - * @names: Names of sysfs files. - * @data: 1 to turn on. - * Returns number of bytes written or a negative error code. - */ -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data) -{ - return inv_sysfs_write((char*)names->power_state, (long)data); -} - - - diff --git a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h b/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h deleted file mode 100644 index 45a35f9..0000000 --- a/libsensors_iio/software/core/mllite/linux/inv_sysfs_utils.h +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @brief Provides helpful file IO wrappers for use with sysfs. - * @details Based on Jonathan Cameron's @e iio_utils.h. - */ - -#ifndef _INV_SYSFS_UTILS_H_ -#define _INV_SYSFS_UTILS_H_ - -/** - * struct inv_sysfs_names_s - Files needed by user applications. - * @buffer: Ring buffer attached to FIFO. - * @enable: Turns on HW-to-ring buffer flow. - * @raw_data: Raw data from registers. - * @temperature: Temperature data from register. - * @fifo_rate: FIFO rate/ODR. - * @power_state: Power state (this is a five-star comment). - * @fsr: Full-scale range. - * @lpf: Digital low pass filter. - * @scale: LSBs / dps (or LSBs / Gs). - * @temp_scale: LSBs / degrees C. - * @temp_offset: Offset in LSBs. - */ -struct inv_sysfs_names_s { - - //Sysfs for ITG3500 & MPU6050 - const char *buffer; - const char *enable; - const char *raw_data; //Raw Gyro data - const char *temperature; - const char *fifo_rate; - const char *power_state; - const char *fsr; - const char *lpf; - const char *scale; //Gyro scale - const char *temp_scale; - const char *temp_offset; - const char *self_test; - //Starting Sysfs available for MPU6050 only - const char *accel_en; - const char *accel_fifo_en; - const char *accel_fs; - const char *clock_source; - const char *early_suspend_en; - const char *firmware_loaded; - const char *gyro_en; - const char *gyro_fifo_en; - const char *key; - const char *raw_accel; - const char *reg_dump; - const char *tap_on; - const char *dmp_firmware; -}; - -/* File IO. Typically won't be called directly by user application, but they'll - * be here for your enjoyment. - */ -int inv_sysfs_write(char *filename, long data); -int inv_sysfs_read(char *filename, long num_bytes, char *data); - -/* Helper APIs to extract specific data. */ -int inv_read_buffer(int fd, long *data, long long *timestamp); -int inv_read_raw(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temperature_raw(const struct inv_sysfs_names_s *names, short *data, - long long *timestamp); -int inv_read_fifo_rate(const struct inv_sysfs_names_s *names, short *data); -int inv_read_power_state(const struct inv_sysfs_names_s *names, char *data); -int inv_read_scale(const struct inv_sysfs_names_s *names, float *data); -int inv_read_temp_scale(const struct inv_sysfs_names_s *names, short *data); -int inv_read_temp_offset(const struct inv_sysfs_names_s *names, short *data); -int inv_write_fifo_rate(const struct inv_sysfs_names_s *names, short data); -int inv_write_buffer_enable(const struct inv_sysfs_names_s *names, char data); -int inv_write_power_state(const struct inv_sysfs_names_s *names, char data); - -/* Scaled data. */ -int inv_read_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); -int inv_read_temp_q16(const struct inv_sysfs_names_s *names, long *data, - long long *timestamp); - - -#endif /* #ifndef _INV_SYSFS_UTILS_H_ */ - - diff --git a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c index f0f078c..91c766b 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c +++ b/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -30,10 +30,10 @@ #define LOADDMP_LOG MPL_LOGI #define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) -#define DMP_CODE_SIZE 3060 +#define DMP_CODE_SIZE 3065 static const unsigned char dmpMemory[DMP_CODE_SIZE] = { - /* bank # 0 */ + /* 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, @@ -212,32 +212,32 @@ static const unsigned char dmpMemory[DMP_CODE_SIZE] = { 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, 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, 0x94, 0x01, 0x29, 0x51, 0x79, + 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 */ - 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 + 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) @@ -250,11 +250,10 @@ inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) if (len <= 0) { MPL_LOGE("Nothing to write"); return INV_ERROR_FILE_WRITE; - } - else { + } else { MPL_LOGI("dmp firmware size to write = %d", len); } - if ( fd == NULL ) { + if (fd == NULL) { return INV_ERROR_FILE_OPEN; } bytesWritten = fwrite(dmp, 1, len, fd); @@ -262,8 +261,7 @@ inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", bytesWritten, len); result = INV_ERROR_FILE_WRITE; - } - else { + } else { MPL_LOGI("Bytes written = %d", bytesWritten); } return result; diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c index c5cf2e6..24b3217 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.c +++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.c @@ -38,26 +38,40 @@ #define STORECAL_LOG MPL_LOGI #define LOADCAL_LOG MPL_LOGI -inv_error_t inv_read_cal(unsigned char *cal, size_t len) +inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) { FILE *fp; - int bytesRead; 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; } - bytesRead = fread(cal, 1, len, fp); - if (bytesRead != len) { - MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", - bytesRead, len); + + // 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); + MPL_LOGI("Bytes read = %d", *bytesRead); } read_cal_end: @@ -261,31 +275,18 @@ inv_error_t inv_store_cal(unsigned char *calData, size_t length) */ inv_error_t inv_load_calibration(void) { - unsigned char *calData; + unsigned char *calData= NULL; inv_error_t result = 0; - size_t length; - - inv_get_mpl_state_size(&length); - if (length <= 0) { - MPL_LOGE("Could not get file calibration length - " - "error %d - aborting\n", result); - return result; - } + size_t bytesRead = 0; - 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_read_cal(calData, length); + 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, length); + result = inv_load_mpl_states(calData, bytesRead); if (result != INV_SUCCESS) { MPL_LOGE("Could not load the calibration data - " "error %d - aborting\n", result); @@ -294,7 +295,7 @@ inv_error_t inv_load_calibration(void) free_mem_n_exit: inv_free(calData); - return INV_SUCCESS; + return result; } /** diff --git a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h index 336f081..115b34c 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_stored_data.h +++ b/libsensors_iio/software/core/mllite/linux/ml_stored_data.h @@ -36,7 +36,7 @@ inv_error_t inv_store_calibration(void); /* Internal APIs */ -inv_error_t inv_read_cal(unsigned char *cal, size_t len); +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); diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c index 5636a02..a12a4ed 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c +++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c @@ -5,7 +5,6 @@ #include #define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" -#define CHIP_NUM 4 enum PROC_SYSFS_CMD { CMD_GET_SYSFS_PATH, CMD_GET_DMP_PATH, @@ -15,7 +14,7 @@ enum PROC_SYSFS_CMD { CMD_GET_DEVICE_NODE }; static char sysfs_path[100]; -static char *chip_name[CHIP_NUM] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050"}; +static char *chip_name[] = {"ITG3500", "MPU6050", "MPU9150", "MPU3050", "MPU6500"}; static int chip_ind; static int initialized =0; static int status = 0; @@ -27,6 +26,8 @@ static int iio_dev_num = 0; #define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" #define FORMAT_TYPE_FILE "%s_type" +#define CHIP_NUM sizeof(chip_name)/sizeof(char*) + static const char *iio_dir = "/sys/bus/iio/devices/"; /** @@ -392,9 +393,9 @@ inv_error_t inv_get_input_number(const char *name, int *num) * name. It should be zeroed before calling this function. * Or it could have unpredicable result. */ -inv_error_t inv_get_iio_trigger_path(const char *name) +inv_error_t inv_get_iio_trigger_path(char *name) { - if (process_sysfs_request(CMD_GET_TRIGGER_PATH, name) < 0) + if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0) return INV_ERROR_NOT_OPENED; else return INV_SUCCESS; @@ -407,9 +408,9 @@ inv_error_t inv_get_iio_trigger_path(const char *name) * node. It should be zeroed before calling this function. * Or it could have unpredicable result. */ -inv_error_t inv_get_iio_device_node(const char *name) +inv_error_t inv_get_iio_device_node(char *name) { - if (process_sysfs_request(CMD_GET_DEVICE_NODE, name) < 0) + if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0) return INV_ERROR_NOT_OPENED; else return INV_SUCCESS; diff --git a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h index eb285c5..9470874 100644 --- a/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h +++ b/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h @@ -27,8 +27,8 @@ 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(const char *name); -inv_error_t inv_get_iio_device_node(const char *name); +inv_error_t inv_get_iio_trigger_path(char *name); +inv_error_t inv_get_iio_device_node(char *name); #ifdef __cplusplus } diff --git a/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/libsensors_iio/software/core/mllite/linux/mlos_linux.c index 75f062e..6c9a6ca 100644 --- a/libsensors_iio/software/core/mllite/linux/mlos_linux.c +++ b/libsensors_iio/software/core/mllite/linux/mlos_linux.c @@ -58,7 +58,9 @@ void *inv_malloc(unsigned int numBytes) inv_error_t inv_free(void *ptr) { // Deallocate space. - free(ptr); + if (ptr) { + free(ptr); + } return INV_SUCCESS; } diff --git a/libsensors_iio/software/core/mllite/ml_math_func.c b/libsensors_iio/software/core/mllite/ml_math_func.c index 86c4b41..d1fd9c4 100644 --- a/libsensors_iio/software/core/mllite/ml_math_func.c +++ b/libsensors_iio/software/core/mllite/ml_math_func.c @@ -648,13 +648,60 @@ void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity } /** find a norm for a vector -* @param[in] a vector [3x1] -* @param[out] output the norm of the input 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) +{ + float divider; + 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/libsensors_iio/software/core/mllite/ml_math_func.h b/libsensors_iio/software/core/mllite/ml_math_func.h index 916de0a..535d849 100644 --- a/libsensors_iio/software/core/mllite/ml_math_func.h +++ b/libsensors_iio/software/core/mllite/ml_math_func.h @@ -24,6 +24,13 @@ 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)); @@ -102,6 +109,11 @@ extern "C" { 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 diff --git a/libsensors_iio/software/core/mllite/mlmath.c b/libsensors_iio/software/core/mllite/mlmath.c deleted file mode 100644 index 5eb4264..0000000 --- a/libsensors_iio/software/core/mllite/mlmath.c +++ /dev/null @@ -1,68 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/******************************************************************************* - * - * $Id: mlmath.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *******************************************************************************/ - -#include - -double ml_asin(double x) -{ - return asin(x); -} - -double ml_atan(double x) -{ - return atan(x); -} - -double ml_atan2(double x, double y) -{ - return atan2(x, y); -} - -double ml_log(double x) -{ - return log(x); -} - -double ml_sqrt(double x) -{ - return sqrt(x); -} - -double ml_ceil(double x) -{ - return ceil(x); -} - -double ml_floor(double x) -{ - return floor(x); -} - -double ml_cos(double x) -{ - return cos(x); -} - -double ml_sin(double x) -{ - return sin(x); -} - -double ml_acos(double x) -{ - return acos(x); -} - -double ml_pow(double x, double y) -{ - return pow(x, y); -} diff --git a/libsensors_iio/software/core/mllite/mpl.c b/libsensors_iio/software/core/mllite/mpl.c index 231cbfd..a8c253d 100644 --- a/libsensors_iio/software/core/mllite/mpl.c +++ b/libsensors_iio/software/core/mllite/mpl.c @@ -41,7 +41,7 @@ inv_error_t inv_init_mpl(void) return INV_SUCCESS; } -const char ml_ver[] = "InvenSense MPL 5.0.1"; +const char ml_ver[] = "InvenSense MPL 5.1.1 RC6"; /** * @brief used to get the MPL version. diff --git a/libsensors_iio/software/core/mllite/results_holder.c b/libsensors_iio/software/core/mllite/results_holder.c index 97ffdec..1484f9b 100644 --- a/libsensors_iio/software/core/mllite/results_holder.c +++ b/libsensors_iio/software/core/mllite/results_holder.c @@ -14,12 +14,12 @@ * @brief Results Holder for HAL. */ #include "results_holder.h" -#include "log.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 diff --git a/libsensors_iio/software/core/mllite/storage_manager.c b/libsensors_iio/software/core/mllite/storage_manager.c index 4b92bfc..721f858 100644 --- a/libsensors_iio/software/core/mllite/storage_manager.c +++ b/libsensors_iio/software/core/mllite/storage_manager.c @@ -4,6 +4,7 @@ See included License.txt for License information. $ */ + /** * @defgroup Storage_Manager storage_manager * @brief Motion Library - Stores Data for functions. diff --git a/libsensors_iio/software/core/mpl/adv_func.h b/libsensors_iio/software/core/mpl/adv_func.h deleted file mode 100644 index 3f8595f..0000000 --- a/libsensors_iio/software/core/mpl/adv_func.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_ADVFUNC_H__ -#define MLDMP_ADVFUNC_H__ -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - float inv_compass_angle(const long *compass, const long *grav, const float *quat); - inv_error_t inv_set_dmp_quaternion(long *q); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_ADVFUNC_H__ diff --git a/libsensors_iio/software/core/mpl/auto_calibration.h b/libsensors_iio/software/core/mpl/auto_calibration.h deleted file mode 100644 index 3dd9827..0000000 --- a/libsensors_iio/software/core/mpl/auto_calibration.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_IN_USE_AUTO_CALIBRATION_H__ -#define MLDMP_IN_USE_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_in_use_auto_calibration_is_enabled(unsigned char *is_enabled); -inv_error_t inv_init_in_use_auto_calibration(void); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_IN_USE_AUTO_CALIBRATION_H__ - diff --git a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so index 116b618..fbd346f 100644 Binary files a/libsensors_iio/software/core/mpl/build/android/libmplmpu.so and b/libsensors_iio/software/core/mpl/build/android/libmplmpu.so differ diff --git a/libsensors_iio/software/core/mpl/build/android/static.mk b/libsensors_iio/software/core/mpl/build/android/static.mk deleted file mode 100644 index 6ad45de..0000000 --- a/libsensors_iio/software/core/mpl/build/android/static.mk +++ /dev/null @@ -1,110 +0,0 @@ -MLLITE_LIB_NAME = mllite -LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(STATIC_LIB_EXT) - -MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) - -OBJFOLDER = $(CURDIR)/obj - -CROSS = arm-none-linux-gnueabi- -COMP= $(CROSS)gcc -LINK= $(CROSS)ar cr - -MLLITE_DIR = $(MLSDK_ROOT)/mllite -MPL_DIR = $(MLSDK_ROOT)/mldmp -MLPLATFORM_DIR = $(MLSDK_ROOT)/platform - -include $(MLSDK_ROOT)/Android-common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -CFLAGS += -Wall -fpic -CFLAGS += -mthumb-interwork -fno-exceptions -ffunction-sections -funwind-tables -fstack-protector -fno-short-enums -fmessage-length=0 -CFLAGS += -DNDEBUG -CFLAGS += -D_REENTRANT -DLINUX -DANDROID -CFLAGS += -I$(MLLITE_DIR) -CFLAGS += -I$(MLPLATFORM_DIR)/include -CFLAGS += -I$(MLSDK_ROOT)/mlutils -CFLAGS += -I$(MLSDK_ROOT)/mlapps/common -CFLAGS += $(MLSDK_INCLUDES) -CFLAGS += $(MLSDK_DEFINES) - -VPATH += $(MLLITE_DIR) -VPATH += $(MLSDK_ROOT)/mlutils -VPATH += $(MLLITE_DIR)/accel -VPATH += $(MLLITE_DIR)/compass -VPATH += $(MLLITE_DIR)/pressure -VPATH += $(MLLITE_DIR)/mlapps/common - -#################################################################################################### -## sources - -ML_LIBS = $(MLPLATFORM_DIR)/linux/$(LIB_PREFIX)$(MLPLATFORM_LIB_NAME).$(STATIC_LIB_EXT) - -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_mpu.c -ML_SOURCES += $(MLLITE_DIR)/mldl_cfg_init_linux.c -ML_SOURCES += $(MLLITE_DIR)/accel.c -ML_SOURCES += $(MLLITE_DIR)/compass.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor.c -ML_SOURCES += $(MLLITE_DIR)/compass_supervisor_adv_callbacks.c -ML_SOURCES += $(MLLITE_DIR)/key0_96.c -ML_SOURCES += $(MLLITE_DIR)/pressure.c -ML_SOURCES += $(MLLITE_DIR)/ml.c -ML_SOURCES += $(MLLITE_DIR)/ml_invobj.c -ML_SOURCES += $(MLLITE_DIR)/ml_init.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_lite.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_adv.c -ML_SOURCES += $(MLLITE_DIR)/mlarray_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlBiasNoMotion.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFO.c -ML_SOURCES += $(MLLITE_DIR)/mlFIFOHW.c -ML_SOURCES += $(MLLITE_DIR)/mlMathFunc.c -ML_SOURCES += $(MLLITE_DIR)/mlcontrol.c -ML_SOURCES += $(MLLITE_DIR)/mldl.c -ML_SOURCES += $(MLLITE_DIR)/mldmp.c -ML_SOURCES += $(MLLITE_DIR)/dmpDefault.c -ML_SOURCES += $(MLLITE_DIR)/mlstates.c -ML_SOURCES += $(MLLITE_DIR)/mlsupervisor.c -ML_SOURCES += $(MLLITE_DIR)/ml_stored_data.c -ML_SOURCES += $(MLLITE_DIR)/ustore_manager.c -ML_SOURCES += $(MLLITE_DIR)/ustore_mlsl_io.c -ML_SOURCES += $(MLLITE_DIR)/ustore_adv_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/ustore_lite_fusion_delegate.c -ML_SOURCES += $(MLLITE_DIR)/temp_comp_legacy.c -ML_SOURCES += $(MLLITE_DIR)/mlSetGyroBias.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/checksum.c -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mputest.c -ML_SOURCES += $(MLLITE_DIR)/mldl_print_cfg.c -ifeq ($(MPU_NAME),MPU3050) -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu3050test.c -else -ML_SOURCES += $(MLSDK_ROOT)/mlutils/mpu6050test.c -endif - -ML_OBJS := $(addsuffix .o,$(ML_SOURCES)) -ML_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(ML_SOURCES)))) - -#################################################################################################### -## rules - -.PHONY: all clean cleanall - -all: $(LIBRARY) $(MK_NAME) - -$(LIBRARY) : $(OBJFOLDER) $(ML_OBJS_DST) $(ML_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n\n") - mkdir obj - -$(ML_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n\n") - $(COMP) $(CFLAGS) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(MLSDK_INCLUDES) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(LIBRARY) $(OBJFOLDER) - diff --git a/libsensors_iio/software/core/mpl/fusion_9axis.h b/libsensors_iio/software/core/mpl/fusion_9axis.h index 616694a..1ba1ebb 100644 --- a/libsensors_iio/software/core/mpl/fusion_9axis.h +++ b/libsensors_iio/software/core/mpl/fusion_9axis.h @@ -26,6 +26,8 @@ extern "C" { 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 } diff --git a/libsensors_iio/software/core/mpl/interpolator.h b/libsensors_iio/software/core/mpl/interpolator.h deleted file mode 100644 index 5eb571d..0000000 --- a/libsensors_iio/software/core/mpl/interpolator.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef INTERPOLATOR_H -#define INTERPOLATOR_H - -#ifdef __cplusplus -extern "C" { -#endif - -//#include "mltypes.h" - - /* ------------ */ - /* - Defines. - */ - /* ------------ */ - -#define MAX_INTERPOLATION (20) - -typedef struct { - long x[2]; - long y[4]; -} tInterpolate2; -typedef struct { - long x[2]; - long y[6]; -} tInterpolate3; -typedef struct { - long x[5]; - long y[10]; - int idx1; -} tInterpolate5; -typedef struct { - tInterpolate2 state1; - tInterpolate2 state2; -} tInterpolate4; -typedef struct { - tInterpolate3 state1; - tInterpolate2 state2; -} tInterpolate6; -typedef struct { - tInterpolate2 state1; - tInterpolate4 state2; -} tInterpolate8; -typedef struct { - tInterpolate3 state1; - tInterpolate3 state2; -} tInterpolate9; -typedef struct { - tInterpolate5 state1; - tInterpolate2 state2; -} tInterpolate10; -typedef struct { - tInterpolate4 state1; - tInterpolate3 state2; -} tInterpolate12; -typedef struct { - tInterpolate5 state1; - tInterpolate3 state2; -} tInterpolate15; -typedef struct { - tInterpolate2 state1; - tInterpolate8 state2; -} tInterpolate16; -typedef struct { - tInterpolate9 state1; - tInterpolate2 state2; -} tInterpolate18; -typedef struct { - tInterpolate10 state1; - tInterpolate2 state2; -} tInterpolate20; - -typedef union { - tInterpolate2 u2; - tInterpolate3 u3; - tInterpolate4 u4; - tInterpolate5 u5; - tInterpolate6 u6; - tInterpolate8 u8; - tInterpolate9 u9; - tInterpolate10 u10; - tInterpolate12 u12; - tInterpolate15 u15; - tInterpolate16 u16; - tInterpolate18 u18; - tInterpolate20 u20; -} tInterpolateState; - - /* --------------------- */ - /* - Function p-types. - */ - /* --------------------- */ -int inv_get_interp_amount( int x ); -int inv_interpolate( int amount, long input, long *output, tInterpolateState *state ); -long inv_fxmult( long x, long y ); - - -#ifdef __cplusplus -} -#endif - -#endif /* INTERPOLATOR_H */ diff --git a/libsensors_iio/software/core/mpl/inv_log.h b/libsensors_iio/software/core/mpl/inv_log.h deleted file mode 100644 index 972844b..0000000 --- a/libsensors_iio/software/core/mpl/inv_log.h +++ /dev/null @@ -1,7 +0,0 @@ -#include "mltypes.h" -#ifndef INV_INV_LOG_H__ -#define INV_INV_LOG_H__ - -#define INV_LOGE(s) - -#endif // INV_INV_LOG_H__ diff --git a/libsensors_iio/software/core/mpl/invensense_adv.h b/libsensors_iio/software/core/mpl/invensense_adv.h index 9e59c18..12932c9 100644 --- a/libsensors_iio/software/core/mpl/invensense_adv.h +++ b/libsensors_iio/software/core/mpl/invensense_adv.h @@ -28,3 +28,4 @@ #include "quaternion_supervisor.h" #include "mag_disturb.h" #include "quat_accuracy_monitor.h" +#include "shake.h" diff --git a/libsensors_iio/software/core/mpl/mlsetinterrupts.h b/libsensors_iio/software/core/mpl/mlsetinterrupts.h deleted file mode 100644 index a81dabb..0000000 --- a/libsensors_iio/software/core/mpl/mlsetinterrupts.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - $License: - Copyright (c) 2008 InvenSense Corporation, All Rights Reserved. - $ - */ - -#ifndef MLSETINTERRUPT_H -#define MLSETINTERRUPT_H - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - - /* DEPRECATED - Scheduled for removal. Do not use */ - inv_error_t MLSetInterrupts(unsigned short interrupts); - -#ifdef __cplusplus -} -#endif - -#endif /* MLSETINTERRUPT_H */ diff --git a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h b/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h deleted file mode 100644 index 3779381..0000000 --- a/libsensors_iio/software/core/mpl/mlsupervisor_9axis.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id: mlsupervisor_9axis.h 6123 2011-09-30 18:21:11Z mcaramello $ - * - *****************************************************************************/ - -#ifndef MLDMP_MLSUPERVISOR_H__ -#define MLDMP_MLSUPERVISOR_H__ - -#include "mltypes.h" - -//#include "temp_comp.h" - -struct inv_fusion_t { - int compassCount; - long quat[4]; -}; - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_enable_9x_fusion(void); -inv_error_t inv_disable_9x_fusion(void); - -inv_error_t inv_enable_9x_fusion_legacy(void); -inv_error_t inv_disable_9x_fusion_legacy(void); - -inv_error_t inv_enable_9x_fusion_new(void); -inv_error_t inv_disable_9x_fusion_new(void); - -inv_error_t inv_enable_9x_fusion_basic(void); -inv_error_t inv_disable_9x_fusion_basic(void); - -inv_error_t inv_enable_9x_fusion_external(void); -inv_error_t inv_disable_9x_fusion_external(void); - -inv_error_t inv_enable_maintain_heading(void); -inv_error_t inv_disable_maintain_heading(void); - -void inv_set_compass_state(long compassState, long accState, - unsigned long deltaTime, - int magDisturb, int gotBias, - int *new_state, - int *new_accuracy); - -#ifdef __cplusplus -} -#endif - -#endif // MLDMP_MLSUPERVISOR_H__ diff --git a/libsensors_iio/software/core/mpl/orientation.h b/libsensors_iio/software/core/mpl/orientation.h deleted file mode 100644 index ab4e45e..0000000 --- a/libsensors_iio/software/core/mpl/orientation.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef MLDMP_ORIENTATION_H__ -#define MLDMP_ORIENTATION_H__ - -#include "mltypes.h" -/*******************************************************************************/ -/* Orientations */ -/*******************************************************************************/ - -#define INV_X_UP 0x01 -#define INV_X_DOWN 0x02 -#define INV_Y_UP 0x04 -#define INV_Y_DOWN 0x08 -#define INV_Z_UP 0x10 -#define INV_Z_DOWN 0x20 -#define INV_ORIENTATION_ALL 0x3F - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_orientation(void); - inv_error_t inv_disable_orientation(void); - inv_error_t inv_set_orientation(int orientation); - inv_error_t inv_set_orientation_cb(void (*callback)(unsigned short)); - inv_error_t inv_get_orientation(int *orientation); - inv_error_t inv_get_orientation_state(int * state); - inv_error_t inv_set_orientation_interrupt(unsigned char on); - inv_error_t inv_set_orientation_thresh(float angle, - float hysteresis, - unsigned long time, - unsigned int axis); - -#ifdef __cplusplus -} -#endif - -#endif // MLDMP_ORIENTATION_H__ diff --git a/libsensors_iio/software/core/mpl/progressive_no_motion.h b/libsensors_iio/software/core/mpl/progressive_no_motion.h deleted file mode 100644 index 99333e3..0000000 --- a/libsensors_iio/software/core/mpl/progressive_no_motion.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id:$ - * - *****************************************************************************/ - -#ifndef MLDMP_PROG_NO_MOTION_H__ -#define MLDMP_PROG_NO_MOTION_H__ - -#include "mltypes.h" - -#define PROG_NO_MOTION 1 -#define PROG_MOTION 2 - -#ifdef __cplusplus -extern "C" { -#endif - -/* APIs */ -inv_error_t inv_enable_prog_no_motion(void); -inv_error_t inv_disable_prog_no_motion(void); - -/* internal use */ -int inv_get_prog_no_motion_enabled(void); -void inv_get_prog_no_motion_bias_changed(void); -int inv_get_prog_no_motion_state(void); - -#ifdef __cplusplus -} -#endif - - -#endif // MLDMP_PROG_NO_MOTION_H__ diff --git a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h index 2cf7a50..3c6a2c1 100644 --- a/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h +++ b/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h @@ -62,6 +62,7 @@ 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 } diff --git a/libsensors_iio/software/core/mpl/sensor_moments.h b/libsensors_iio/software/core/mpl/sensor_moments.h deleted file mode 100644 index 73eb363..0000000 --- a/libsensors_iio/software/core/mpl/sensor_moments.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ - -/****************************************************************************** - * - * $Id$ - * - *****************************************************************************/ - -#ifndef MLDMP_SENSOR_MOMENTS_H__ -#define MLDMP_SENSOR_MOMENTS_H__ - -#include "mltypes.h" - -enum moment_ord { - SECOND_ORD=0, - THIRD_ORD, - FOURTH_ORD, - MAX_ORD -}; - -#ifdef __cplusplus -extern "C" { -#endif - - inv_error_t inv_enable_sm(void); - inv_error_t inv_disable_sm(void); - inv_error_t inv_sm_record_data(float sample, void *sensor); - inv_error_t inv_sm_update_evt_act_state(int motion); - void *inv_init_sm(enum moment_ord); - float inv_sm_get_filtered_data(void *sensor); - -#ifdef __cplusplus -} -#endif - -#endif // MLDMP_SENSOR_MOMENTS_H__ - diff --git a/libsensors_iio/software/core/mpl/shake.h b/libsensors_iio/software/core/mpl/shake.h new file mode 100644 index 0000000..67acb7b --- /dev/null +++ b/libsensors_iio/software/core/mpl/shake.h @@ -0,0 +1,94 @@ +/* + $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/libsensors_iio/software/core/mpl/state_storage.h b/libsensors_iio/software/core/mpl/state_storage.h deleted file mode 100644 index c1eb47b..0000000 --- a/libsensors_iio/software/core/mpl/state_storage.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -#ifndef INV_STATE_STORAGE_H__ -#define INV_STATE_STORAGE_H__ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -inv_error_t inv_store_data(const void *data, size_t size, unsigned long module, - unsigned long version); -inv_error_t inv_load_data(void *data, size_t size, unsigned long module, - unsigned long version); - -#ifdef __cplusplus -} -#endif - -#endif // INV_STATE_STORAGE_H__ diff --git a/libsensors_iio/software/simple_apps/common/external_hardware.h b/libsensors_iio/software/simple_apps/common/external_hardware.h deleted file mode 100644 index 55e3b20..0000000 --- a/libsensors_iio/software/simple_apps/common/external_hardware.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - Accelerometer -*/ -#define get_accel_slave_descr NULL - -#ifdef CONFIG_MPU_SENSORS_ADXL34X /* ADI accelerometer */ -struct ext_slave_descr *adxl34x_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr adxl34x_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_BMA150 /* Bosch accelerometer */ -struct ext_slave_descr *bma150_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr bma150_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_BMA222 /* Bosch 222 accelerometer */ -struct ext_slave_descr *bma222_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr bma222_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_BMA250 /* Bosch accelerometer */ -struct ext_slave_descr *bma250_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr bma250_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_KXSD9 /* Kionix accelerometer */ -struct ext_slave_descr *kxsd9_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr kxsd9_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_KXTF9 /* Kionix accelerometer */ -struct ext_slave_descr *kxtf9_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr kxtf9_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_LIS331DLH /* ST accelerometer */ -struct ext_slave_descr *lis331_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr lis331_get_slave_descr -#endif - - -#ifdef CONFIG_MPU_SENSORS_LIS3DH /* ST accelerometer */ -struct ext_slave_descr *lis3dh_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr lis3dh_get_slave_descr -#endif - -/* ST accelerometer in LSM303DLx combo */ -#if defined CONFIG_MPU_SENSORS_LSM303DLX_A -struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr lsm303dlx_a_get_slave_descr -#endif - -/* MPU6050 Accel */ -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ - defined CONFIG_MPU_SENSORS_MPU6050B1 -struct ext_slave_descr *mpu6050_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr mpu6050_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_MMA8450 /* Freescale accelerometer */ -struct ext_slave_descr *mma8450_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr mma8450_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_MMA845X /* Freescale accelerometer */ -struct ext_slave_descr *mma845x_get_slave_descr(void); -#undef get_accel_slave_descr -#define get_accel_slave_descr mma845x_get_slave_descr -#endif - - -/* - Compass -*/ -#define get_compass_slave_descr NULL - -#ifdef CONFIG_MPU_SENSORS_AK8975 /* AKM compass */ -struct ext_slave_descr *ak8975_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr ak8975_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_AMI30X /* AICHI Steel AMI304/305 compass */ -struct ext_slave_descr *ami30x_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr ami30x_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_AMI306 /* AICHI Steel AMI306 compass */ -struct ext_slave_descr *ami306_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr ami306_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_HMC5883 /* Honeywell compass */ -struct ext_slave_descr *hmc5883_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr hmc5883_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_MMC314X /* MEMSIC compass */ -struct ext_slave_descr *mmc314x_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr mmc314x_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_LSM303DLX_M /* ST compass */ -struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr lsm303dlx_m_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_YAS529 /* Yamaha compass */ -struct ext_slave_descr *yas529_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr yas529_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_YAS530 /* Yamaha compass */ -struct ext_slave_descr *yas530_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr yas530_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_HSCDTD002B /* Alps HSCDTD002B compass */ -struct ext_slave_descr *hscdtd002b_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr hscdtd002b_get_slave_descr -#endif - -#ifdef CONFIG_MPU_SENSORS_HSCDTD004A /* Alps HSCDTD004A compass */ -struct ext_slave_descr *hscdtd004a_get_slave_descr(void); -#undef get_compass_slave_descr -#define get_compass_slave_descr hscdtd004a_get_slave_descr -#endif -/* - Pressure -*/ -#define get_pressure_slave_descr NULL - -#ifdef CONFIG_MPU_SENSORS_BMA085 /* BMA pressure */ -struct ext_slave_descr *bma085_get_slave_descr(void); -#undef get_pressure_slave_descr -#define get_pressure_slave_descr bma085_get_slave_descr -#endif diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.c b/libsensors_iio/software/simple_apps/common/fopenCMake.c deleted file mode 100644 index 2936109..0000000 --- a/libsensors_iio/software/simple_apps/common/fopenCMake.c +++ /dev/null @@ -1,56 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/****************************************************************************** - * - * $Id: fopenCMake.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *****************************************************************************/ - -#include - -#include "fopenCMake.h" -#include "path_configure.h" - -/** - * @brief Replacement for fopen that concatenates the location of the - * source tree onto the filename path. - * It looks in 3 locations: - * - in the current directory, - * - then it looks in "..", - * - lastly in the define UNITTEST_SOURCE_DIR which - * gets defined by CMake. - * @param filename - * Filename relative to base of source directory. - * @param prop - * Second argument to fopen. - */ -FILE *fopenCMake(const char *filename, const char *prop) -{ - char path[150]; - FILE *file; - - // Look first in current directory - file = fopen(filename, prop); - if (file == NULL) { - // Now look in ".." -#ifdef WIN32 - strcpy(path, "..\\"); -#else - strcpy(path, "../"); -#endif - strcat(path, filename); - file = fopen(path, prop); - if (file == NULL) { - // Now look in definition by CMake - strcpy(path, PATH_SOURCE_DIR); - strcat(path, filename); - file = fopen(path, prop); - } - } - return file; -} - - diff --git a/libsensors_iio/software/simple_apps/common/fopenCMake.h b/libsensors_iio/software/simple_apps/common/fopenCMake.h deleted file mode 100644 index c5eba39..0000000 --- a/libsensors_iio/software/simple_apps/common/fopenCMake.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef FOPEN_CMAKE_H__ -#define FOPEN_CMAKE_H__ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -FILE *fopenCMake( const char *filename, const char *prop ); - -#ifdef __cplusplus -} -#endif - -#endif // FOPEN_CMAKE_H__ diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.c b/libsensors_iio/software/simple_apps/common/gestureMenu.c deleted file mode 100644 index 2a9487c..0000000 --- a/libsensors_iio/software/simple_apps/common/gestureMenu.c +++ /dev/null @@ -1,725 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * $Id: gestureMenu.c 5705 2011-06-28 19:32:29Z nroyer $ - *****************************************************************************/ - -#define _USE_MATH_DEFINES -#include -#include -#include -#include - -#include "ml.h" -#include "mlmath.h" -#include "gesture.h" -#include "orientation.h" -#include "gestureMenu.h" -#include "fifo.h" - -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "gest" -#include "log.h" -#include "mldl_cfg.h" - -static unsigned long sensors[] = { - INV_NINE_AXIS, - INV_THREE_AXIS_GYRO, - INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL, - INV_THREE_AXIS_ACCEL, - INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS, - INV_THREE_AXIS_COMPASS, - INV_SIX_AXIS_GYRO_ACCEL, - INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS, - INV_SIX_AXIS_ACCEL_COMPASS, -}; - -static char *sensors_string[] = { - "INV_NINE_AXIS", - "INV_THREE_AXIS_GYRO", - "INV_DMP_PROCESSOR | INV_THREE_AXIS_ACCEL", - "INV_THREE_AXIS_ACCEL", - "INV_DMP_PROCESSOR | INV_THREE_AXIS_COMPASS", - "INV_THREE_AXIS_COMPASS", - "INV_SIX_AXIS_GYRO_ACCEL", - "INV_DMP_PROCESSOR | INV_SIX_AXIS_ACCEL_COMPASS", - "INV_SIX_AXIS_ACCEL_COMPASS", -}; - -/** - * Prints the menu with the current thresholds - * - * @param params The parameters to print - */ -void PrintGestureMenu(tGestureMenuParams const * const params) -{ - MPL_LOGI("Press h at any time to re-display this menu\n"); - MPL_LOGI("TAP PARAMETERS:\n"); - MPL_LOGI(" Use LEFT and RIGHT arrows to adjust Tap Time \n\n"); - MPL_LOGI(" j : Increase X threshold : %5d\n", - params->xTapThreshold); - MPL_LOGI(" J (Shift-j): Decrease X threshold\n"); - MPL_LOGI(" k : Increase Y threshold : %5d\n", - params->yTapThreshold); - MPL_LOGI(" K (Shift-k): Decrease Y threshold\n"); - MPL_LOGI(" i : Increase Z threshold : %5d\n", - params->zTapThreshold); - MPL_LOGI(" I (Shift-i): Decrease Z threshold\n"); - MPL_LOGI(" l : Increase tap time : %5d\n", - params->tapTime); - MPL_LOGI(" L (Shift-l): Decrease tap time\n"); - MPL_LOGI(" o : Increase next tap time : %5d\n", - params->nextTapTime); - MPL_LOGI(" O (Shift-o): Increase next tap time\n"); - MPL_LOGI(" u : Increase max Taps : %5d\n", - params->maxTaps); - MPL_LOGI(" U (Shift-u): Increase max Taps\n"); - - MPL_LOGI("SHAKE PARAMETERS:\n"); - MPL_LOGI(" x : Increase X threshold : %5d\n", - params->xShakeThresh); - MPL_LOGI(" X (Shift-x): Decrease X threshold\n"); - MPL_LOGI(" y : Increase Y threshold : %5d\n", - params->yShakeThresh); - MPL_LOGI(" Y (Shift-y): Decrease Y threshold\n"); - MPL_LOGI(" z : Increase Z threshold : %5d\n", - params->zShakeThresh); - MPL_LOGI(" Z (Shift-z): Decrease Z threshold\n"); - MPL_LOGI(" s : Toggle Shake Function : %5d\n", - params->shakeFunction); - MPL_LOGI(" t : Increase Shake Time : %5d\n", - params->shakeTime); - MPL_LOGI(" T (Shift-T): Decrease Shake Time\n"); - MPL_LOGI(" n : Increase Next Shake Time : %5d\n", - params->nextShakeTime); - MPL_LOGI(" N (Shift-n): Decrease Next Shake Time\n"); - MPL_LOGI(" m : Increase max Shakes : %5d\n", - params->maxShakes); - MPL_LOGI(" M (Shift-m): Decrease max Shakes\n"); - MPL_LOGI("SNAP PARAMETERS:\n"); - MPL_LOGI(" p : Increase Pitch threshold : %5d\n", - params->xSnapThresh); - MPL_LOGI(" P (Shift-p): Decrease Pitch threshold\n"); - MPL_LOGI(" r : Increase Roll threshold : %5d\n", - params->ySnapThresh); - MPL_LOGI(" R (Shift-r): Decrease Roll threshold\n"); - MPL_LOGI(" a : Increase yAw threshold : %5d\n", - params->zSnapThresh); - MPL_LOGI(" A (Shift-a): Decrease yAw threshold\n"); - MPL_LOGI("YAW ROTATION PARAMETERS:\n"); - MPL_LOGI(" e : Increase yaW Rotate time : %5d\n", - params->yawRotateTime); - MPL_LOGI(" E (Shift-r): Decrease yaW Rotate time\n"); - MPL_LOGI(" w : Increase yaW Rotate threshold : %5d\n", - params->yawRotateThreshold); - MPL_LOGI(" W (Shift-w): Decrease yaW Rotate threshold\n"); - MPL_LOGI("ORIENTATION PARAMETER:\n"); - MPL_LOGI(" d : Increase orientation angle threshold : %5f\n", - params->orientationThreshold); - MPL_LOGI(" D (Shift-d): Decrease orientation angle threshold\n"); - MPL_LOGI("FIFO RATE:\n"); - MPL_LOGI(" f : Increase fifo divider : %5d\n", - inv_get_fifo_rate()); - MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); - MPL_LOGI("REQUESTED SENSORS:\n"); - MPL_LOGI(" S (Shift-s): Toggle in use sensors : %s\n", - sensors_string[params->sensorsIndex]); - MPL_LOGI(" F (Shift-f): Decrease fifo divider\n"); - - /* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */ - /* S is available */ - - MPL_LOGI("\n\n"); -} - -/** - * Handles a keyboard input and updates an appropriate threshold, prints then - * menu or returns false if the character is not processed. - * - * @param params The parameters to modify if the thresholds are updated - * @param ch The input character - * - * @return true if the character was processed, false otherwise - */ -inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params, char ch) -{ - int result = INV_SUCCESS; - /* Dynamic keyboard processing */ - - switch (ch) { - case 'j': - params->xTapThreshold += 20; - // Intentionally fall through - case 'J': { - params->xTapThreshold -= 10; - if (params->xTapThreshold < 0) params->xTapThreshold = 0; - result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("MLSetTapThresh returned :%d\n", result); - } - MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_X, %d)\n", - params->xTapThreshold); - } break; - case 'k': - params->yTapThreshold += 20; - // Intentionally fall through - case 'K': { - params->yTapThreshold -= 10; - if (params->yTapThreshold < 0) params->yTapThreshold = 0; - result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("MLSetTapThresh returned :%d\n", result); - } - MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Y, %d)\n", - params->yTapThreshold); - } break; - case 'i': - params->zTapThreshold += 20; - // Intentionally fall through - case 'I': { - params->zTapThreshold -= 10; - if (params->zTapThreshold < 0) params->zTapThreshold = 0; - result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("MLSetTapThresh returned :%d\n", result); - } - MPL_LOGI("inv_set_tap_threshold(INV_TAP_AXIS_Z, %d)\n", - params->zTapThreshold); - } break; - - case 'l': - params->tapTime += 20; - // Intentionally fall through - case 'L': { - params->tapTime -= 10; - if (params->tapTime < 0) params->tapTime = 0; - result = inv_set_next_tap_time(params->tapTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); - } - MPL_LOGI("inv_set_next_tap_time(%d)\n", params->tapTime); - } break; - case 'o': - params->nextTapTime += 20; - // Intentionally fall through - case 'O': { - params->nextTapTime -= 10; - if (params->nextTapTime < 0) params->nextTapTime = 0; - result = MLSetNextTapTime(params->nextTapTime); - if (INV_SUCCESS != result) { - MPL_LOGE("MLSetNextTapTime returned :%d\n", result); - } - MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime); - } break; - case 'u': - params->maxTaps += 2; - // Intentionally fall through - case 'U': { - params->maxTaps -= 1; - if (params->maxTaps < 0) params->maxTaps = 0; - result = inv_set_max_taps(params->maxTaps); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_max_taps returned :%d\n", result); - } - MPL_LOGI("inv_set_max_taps(%d)\n", params->maxTaps); - } break; - case 's': { - int shakeParam; - params->shakeFunction = (params->shakeFunction + 1) % 2; - switch (params->shakeFunction) - { - case 0: - shakeParam = INV_NO_RETRACTION; - MPL_LOGE("inv_set_shake_func(INV_NO_RETRACTION)\n"); - break; - case 1: - shakeParam = INV_RETRACTION; - MPL_LOGI("inv_set_shake_func(INV_RETRACTION)\n"); - break; - }; - result = inv_set_shake_func(shakeParam); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_func returned :%d\n", result); - } - } break; - case 'x': - params->xShakeThresh += 200; - // Intentionally fall through - case 'X': { - params->xShakeThresh -= 100; - result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->xShakeThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_shake_thresh(INV_PITCH_SHAKE, %d)\n", params->xShakeThresh); - } break; - case 'y': - params->yShakeThresh += 200; - // Intentionally fall through - case 'Y': { - params->yShakeThresh -= 100; - result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->yShakeThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_shake_thresh(INV_ROLL_SHAKE, %d)\n", params->yShakeThresh); - } break; - case 'z': - params->zShakeThresh += 200; - // Intentionally fall through - case 'Z':{ - params->zShakeThresh -= 100; - result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zShakeThresh); - } break; - case 'r': - params->ySnapThresh += 20; - // Intentionally fall through - case 'R': { - params->ySnapThresh -= 10; - result = inv_set_hard_shake_thresh(INV_ROLL_SHAKE, params->ySnapThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_hard_shake_thresh(INV_ROLL_SHAKE, %d)\n",params->ySnapThresh); - } break; - case 'p': - params->xSnapThresh += 20; - // Intentionally fall through - case 'P': { - params->xSnapThresh -= 10; - result = inv_set_hard_shake_thresh(INV_PITCH_SHAKE, params->xSnapThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_hard_shake_thresh(INV_PITCH_SHAKE, %d)\n", - params->xSnapThresh); - } break; - case 'a': - params->zSnapThresh += 20; - case 'A': { - params->zSnapThresh -= 10; - result = inv_set_hard_shake_thresh(INV_YAW_SHAKE, params->zSnapThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_hard_shake_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_hard_shake_thresh(INV_YAW_SHAKE, %d)\n",params->zSnapThresh); - } break; - - case 't': - params->shakeTime += 20; - case 'T':{ - params->shakeTime -= 10; - result = inv_set_shake_time(params->shakeTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_time returned :%d\n", result); - } - MPL_LOGI("inv_set_shake_time(%d)\n", params->shakeTime); - } break; - case 'n': - params->nextShakeTime += 20; - case 'N':{ - params->nextShakeTime -= 10; - result = inv_set_next_shake_time(params->nextShakeTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); - } - MPL_LOGI("inv_set_next_shake_time(%d)\n", params->nextShakeTime); - } break; - case 'm': - params->maxShakes += 2; - case 'M':{ - params->maxShakes -= 1; - result = inv_set_max_shakes(INV_SHAKE_ALL, params->maxShakes); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_max_shakes returned :%d\n", result); - } - MPL_LOGI("inv_set_max_shakes(%d)\n", params->maxShakes); - } break; - case 'e': - params->yawRotateTime += 20; - case 'E':{ - params->yawRotateTime -= 10; - result = inv_set_yaw_rotate_time(params->yawRotateTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); - } - MPL_LOGI("inv_set_yaw_rotate_time(%d)\n", params->yawRotateTime); - } break; - case 'w': - params->yawRotateThreshold += 2; - case 'W':{ - params->yawRotateThreshold -= 1; - result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_yaw_rotate_thresh(%d)\n", params->yawRotateThreshold); - } break; - case 'c': - params->shakeRejectValue += 0.20f; - case 'C':{ - params->shakeRejectValue -= 0.10f; - result = inv_set_tap_shake_reject(params->shakeRejectValue); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); - } - MPL_LOGI("inv_set_tap_shake_reject(%f)\n", params->shakeRejectValue); - } break; - case 'd': - params->orientationThreshold += 10; - case 'D':{ - params->orientationThreshold -= 5; - if (params->orientationThreshold > 90) { - params->orientationThreshold = 90; - } - - if (params->orientationThreshold < 0 ) { - params->orientationThreshold = 0; - } - - result = inv_set_orientation_thresh(params->orientationThreshold, - 5, 80, - INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_orientation_thresh returned :%d\n", result); - } - MPL_LOGI("inv_set_orientation_thresh(%f, %d, %d," - " INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS)\n", - params->orientationThreshold, 5, 80); - } break; - case 'f': - result = inv_set_fifo_rate(inv_get_fifo_rate() + 1); - MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); - break; - case 'F': - { - unsigned short newRate = inv_get_fifo_rate(); - if (newRate > 0) - newRate--; - result = inv_set_fifo_rate(newRate); - MPL_LOGI("inv_set_fifo_rate(%d)\n",inv_get_fifo_rate()); - break; - } - case 'S': - params->sensorsIndex++; - if (params->sensorsIndex >= ARRAY_SIZE(sensors)) { - params->sensorsIndex = 0; - } - result = inv_set_mpu_sensors( - sensors[params->sensorsIndex] & params->available_sensors); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - MPL_LOGI("%d = inv_set_mpu_sensors(%s)\n", result, - sensors_string[params->sensorsIndex]); - break; - case 'h': - case 'H': { - PrintGestureMenu(params); - } break; - default: { - result = INV_ERROR; - } break; - }; - return result; -} - -/** - * Initializes the tGestureMenuParams to a set of defaults. - * - * @param params The parameters to initialize. - */ -void GestureMenuSetDefaults(tGestureMenuParams * const params) -{ - params->xTapThreshold = 100; - params->yTapThreshold = 100; - params->zTapThreshold = 100; - params->tapTime = 100; - params->nextTapTime = 600; - params->maxTaps = 2; - params->shakeRejectValue = 0.8f; - params->xShakeThresh = 750; - params->yShakeThresh = 750; - params->zShakeThresh = 750; - params->xSnapThresh = 160; - params->ySnapThresh = 320; - params->zSnapThresh = 160; - params->shakeTime = 100; - params->nextShakeTime = 1000; - params->shakeFunction = 0; - params->maxShakes = 3; - params->yawRotateTime = 80; - params->yawRotateThreshold = 70; - params->orientationThreshold = 60; - params->sensorsIndex = 0; - params->available_sensors = INV_NINE_AXIS; -} - -void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, - unsigned long available_sensors) -{ - params->available_sensors = available_sensors; -} -/** - * Call the appropriate MPL set threshold functions and checkes the error codes - * - * @param params The parametrs to use in setting the thresholds - * - * @return INV_SUCCESS or the first error code encountered. - */ -inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params) -{ - inv_error_t result = INV_SUCCESS; - - result = inv_set_tap_threshold(INV_TAP_AXIS_X, params->xTapThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); - return result; - } - result = inv_set_tap_threshold(INV_TAP_AXIS_Y, params->yTapThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); - return result; - } - result = inv_set_tap_threshold(INV_TAP_AXIS_Z, params->zTapThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_tap_threshold returned :%d\n", result); - return result; - } - result = inv_set_next_tap_time(params->tapTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_next_tap_time returned :%d\n", result); - return result; - } - result = MLSetNextTapTime(params->nextTapTime); - if (INV_SUCCESS != result) { - MPL_LOGE("MLSetNextTapTime returned :%d\n", result); - return result; - } - result = inv_set_max_taps(params->maxTaps); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_max_taps returned :%d\n", result); - return result; - } - result = inv_set_tap_shake_reject(params->shakeRejectValue); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_tap_shake_reject returned :%d\n", result); - return result; - } - - //Set up shake gesture - result = inv_set_shake_func(params->shakeFunction); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_func returned :%d\n", result); - return result; - } - result = inv_set_shake_thresh(INV_ROLL_SHAKE, params->xShakeThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); - return result; - } - result = inv_set_shake_thresh(INV_PITCH_SHAKE, params->yShakeThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); - return result; - } - result = inv_set_shake_thresh(INV_YAW_SHAKE, params->zShakeThresh); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_thresh returned :%d\n", result); - return result; - } - result = inv_set_shake_time(params->shakeTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_shake_time returned :%d\n", result); - return result; - } - result = inv_set_next_shake_time(params->nextShakeTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_next_shake_time returned :%d\n", result); - return result; - } - result = inv_set_max_shakes(INV_SHAKE_ALL,params->maxShakes); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_max_shakes returned :%d\n", result); - return result; - } - - // Yaw rotate settings - result = inv_set_yaw_rotate_time(params->yawRotateTime); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_yaw_rotate_time returned :%d\n", result); - return result; - } - result = inv_set_yaw_rotate_thresh(params->yawRotateThreshold); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_yaw_rotate_thresh returned :%d\n", result); - return result; - } - - // Orientation settings - result = inv_set_orientation_thresh(params->orientationThreshold, 5, 80, - INV_X_AXIS | INV_Y_AXIS | INV_Z_AXIS); - if (INV_SUCCESS != result) { - MPL_LOGE("inv_set_orientation_thresh returned: %d\n", result); - return result; - } - - // Requested Sensors - result = inv_set_mpu_sensors( - sensors[params->sensorsIndex] & params->available_sensors); - if (INV_SUCCESS != result) { - MPL_LOGE("MLSetMPUSesnors returned: %d %lx\n", result, - sensors[params->sensorsIndex] & params->available_sensors); - return result; - } - - return INV_SUCCESS; -} - -void PrintGesture(tGesture* gesture) -{ - float speed; - char type[1024]; - switch (gesture->type) - { - case INV_TAP: - { - if (gesture->meta < 0) { - snprintf(type,sizeof(type),"-"); - } else { - snprintf(type,sizeof(type),"+"); - } - - switch (ABS(gesture->meta)) - { - case 1: - strcat(type,"X"); - break; - case 2: - strcat(type,"Y"); - break; - case 3: - strcat(type,"Z"); - break; - default: - strcat(type,"ERROR"); - break; - }; - MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n", - type, - gesture->num, - gesture->strength, - gesture->speed, - gesture->reserved, - (180 / M_PI) * atan2( - (float)gesture->strength, (float)gesture->speed), - (180 / M_PI) * atan2( - (float)gesture->speed, (float)gesture->reserved), - (180 / M_PI) * atan2( - (float)gesture->strength, (float)gesture->reserved) - ); - } - break; - case INV_ROLL_SHAKE: - case INV_PITCH_SHAKE: - case INV_YAW_SHAKE: - { - if (gesture->strength){ - snprintf(type, sizeof(type), "Snap : "); - } else { - snprintf(type, sizeof(type), "Shake: "); - } - - if (gesture->meta==0) { - strcat(type, "+"); - } else { - strcat(type, "-"); - } - - if (gesture->type == INV_ROLL_SHAKE) { - strcat(type, "Roll "); - } else if (gesture->type == INV_PITCH_SHAKE) { - strcat(type, "Pitch "); - } else if (gesture->type == INV_YAW_SHAKE) { - strcat(type, "Yaw "); - } - - speed = (float)gesture->speed + - (float)(gesture->reserved / (float)(1 << 16)); - MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed); - } - break; - case INV_YAW_IMAGE_ROTATE: - { - if (gesture->meta == 0) { - snprintf(type, sizeof(type), "Positive "); - } else { - snprintf(type, sizeof(type), "Negative "); - } - MPL_LOGI("%s Yaw Image Rotation\n", type); - } - break; - default: - MPL_LOGE("Unknown Gesture received\n"); - break; - } -} - -/** - * Prints the new or current orientation using MPL_LOGI and remembers the last - * orientation to print orientation flips. - * - * @param orientation the new or current orientation. 0 to reset. - */ -void PrintOrientation(unsigned short orientation) -{ - // Determine if it was a flip - static int sLastOrientation = 0; - int flip = orientation | sLastOrientation; - - if ((INV_X_UP | INV_X_DOWN) == flip) { - MPL_LOGI("Flip about the X Axis: \n"); - } else if ((INV_Y_UP | INV_Y_DOWN) == flip) { - MPL_LOGI("Flip about the Y axis: \n"); - } else if ((INV_Z_UP | INV_Z_DOWN) == flip) { - MPL_LOGI("Flip about the Z axis: \n"); - } - sLastOrientation = orientation; - - switch (orientation) { - case INV_X_UP: - MPL_LOGI("X Axis is up\n"); - break; - case INV_X_DOWN: - MPL_LOGI("X Axis is down\n"); - break; - case INV_Y_UP: - MPL_LOGI("Y Axis is up\n"); - break; - case INV_Y_DOWN: - MPL_LOGI("Y Axis is down\n"); - break; - case INV_Z_UP: - MPL_LOGI("Z Axis is up\n"); - break; - case INV_Z_DOWN: - MPL_LOGI("Z Axis is down\n"); - break; - case 0: - break; /* Not an error. Resets sLastOrientation */ - default: - MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation); - break; - } -} - - diff --git a/libsensors_iio/software/simple_apps/common/gestureMenu.h b/libsensors_iio/software/simple_apps/common/gestureMenu.h deleted file mode 100644 index 8f804e1..0000000 --- a/libsensors_iio/software/simple_apps/common/gestureMenu.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/***************************************************************************** * - * $Id: gestureMenu.h 5705 2011-06-28 19:32:29Z nroyer $ - ******************************************************************************/ -/** - * @defgroup - * @brief - * - * @{ - * @file gestureMenu.h - * @brief - * - * - */ - -#ifndef __GESTUREMENU_H__ -#define __GESTUREMENU_H__ - -#ifdef __cplusplus -extern "C" { -#endif - -/******************************************************************************/ - typedef struct sGestureMenuParams { - /* Tap Params */ - int xTapThreshold; - int yTapThreshold; - int zTapThreshold; - int tapTime; - int nextTapTime; - int maxTaps; - float shakeRejectValue; - - /* Shake Params */ - int xShakeThresh; - int yShakeThresh; - int zShakeThresh; - int xSnapThresh; - int ySnapThresh; - int zSnapThresh; - int shakeTime; - int nextShakeTime; - int shakeFunction; - int maxShakes; - - /* Yaw rotate params */ - int yawRotateTime; - int yawRotateThreshold; - - /* Orientation */ - float orientationThreshold; - int sensorsIndex; - unsigned long available_sensors; - } tGestureMenuParams; - - void PrintGestureMenu(tGestureMenuParams const * const params) ; - inv_error_t GestureMenuProcessChar(tGestureMenuParams * const params,char ch); - void PrintGesture(gesture_t* gesture); - void PrintOrientation(unsigned short orientation); - void GestureMenuSetDefaults(tGestureMenuParams * const params); - void GestureMenuSetAvailableSensors(tGestureMenuParams * const params, - unsigned long available_sensors); - inv_error_t GestureMenuSetMpl(tGestureMenuParams const * const params); - -/******************************************************************************/ - -#ifdef __cplusplus -} -#endif - -#endif // __GESTUREMENU_H__ diff --git a/libsensors_iio/software/simple_apps/common/helper.c b/libsensors_iio/software/simple_apps/common/helper.c deleted file mode 100644 index 4d634bd..0000000 --- a/libsensors_iio/software/simple_apps/common/helper.c +++ /dev/null @@ -1,110 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/******************************************************************************* - * - * $Id: helper.c 4367 2010-12-21 03:02:55Z prao $ - * - *******************************************************************************/ - -#include -#ifdef _WIN32 -#include -#include -#endif -#ifdef LINUX -#include -#endif -#include -#include - -#include "ml.h" -#include "slave.h" -#include "mldl.h" -#include "mltypes.h" -#include "mlstates.h" -#include "compass.h" - -#include "mlsl.h" -#include "ml.h" - -#include "helper.h" -#include "mlsetup.h" -#include "fopenCMake.h" -#include "int.h" -#include "mlos.h" - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-helper" - -#ifdef AIO -extern inv_error_t MLSLSetYamahaCompassDataMode(unsigned char mode); -#endif - -// Keyboard hit function -int ConsoleKbhit(void) -{ -#ifdef _WIN32 - return _kbhit(); -#else - struct timeval tv; - fd_set read_fd; - - tv.tv_sec=0; - tv.tv_usec=0; - FD_ZERO(&read_fd); - FD_SET(0,&read_fd); - - if(select(1, &read_fd, NULL, NULL, &tv) == -1) - return 0; - - if(FD_ISSET(0,&read_fd)) - return 1; - - return 0; -#endif -} - -char ConsoleGetChar(void) { -#ifdef _WIN32 - return _getch(); -#else - return getchar(); -#endif -} -struct mpuirq_data** InterruptPoll(int *handles, int numHandles, long tv_sec, long tv_usec) -{ - struct mpuirq_data **data; - void *tmp; - int ii; - const int irq_data_size = sizeof(**data) * numHandles + - sizeof(*data) * numHandles; - - tmp = (void *)inv_malloc(irq_data_size); - memset(tmp, 0, irq_data_size); - data = (struct mpuirq_data **)tmp; - for (ii = 0; ii < numHandles; ii++) { - data[ii] = (struct mpuirq_data *)((unsigned long)tmp + - (sizeof(*data) * numHandles) + sizeof(**data) * ii); - } - - if (IntProcess(handles, numHandles, data, tv_sec, tv_usec) > 0) { - for (ii = 0; ii < numHandles; ii++) { - if (data[ii]->interruptcount) { - inv_interrupt_handler(ii); - } - } - } - - /* Return data incase the application needs to look at the timestamp or - other part of the data */ - return data; -} - -void InterruptPollDone(struct mpuirq_data ** data) -{ - inv_free(data); -} diff --git a/libsensors_iio/software/simple_apps/common/helper.h b/libsensors_iio/software/simple_apps/common/helper.h deleted file mode 100644 index b2da520..0000000 --- a/libsensors_iio/software/simple_apps/common/helper.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id: helper-customer.h 5770 2011-07-14 01:34:10Z mcaramello $ - * - *******************************************************************************/ - -#ifndef HELPER_C_H -#define HELPER_C_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "mltypes.h" -#include "mlerrorcode.h" - -/* - Defines -*/ - -#define CALL_N_CHECK(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - } \ -} - -#define CALL_CHECK_N_RETURN_ERROR(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - return r35uLt; \ - } \ -} - -// for functions returning void -#define CALL_CHECK_N_RETURN(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - return; \ - } \ -} - -#define CALL_CHECK_N_EXIT(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - exit (r35uLt); \ - } \ -} - - -#define CALL_CHECK_N_CALLBACK(f, cb) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - cb; \ - } \ -} - -#define CALL_CHECK_N_GOTO(f, label) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - printf("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - goto label; \ - } \ -} - -#define DEFAULT_PLATFORM PLATFORM_ID_MSB_V2 -#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 -#define DEFAULT_COMPASS_ID COMPASS_ID_AK8975 - -#define DataLogger(x) NULL -#define DataLoggerSelector(x) // -#define DataLoggerCb(x) NULL -#define findComm() (9) -#define MenuHwChoice(p,a,c) (*p = DEFAULT_PLATFORM, *a = DEFAULT_ACCEL_ID, \ - *c = DEFAULT_COMPASS_ID, INV_ERROR) - - char ConsoleGetChar(void); - int ConsoleKbhit(void); - struct mpuirq_data **InterruptPoll( - int *handles, int numHandles, long tv_sec, long tv_usec); - void InterruptPollDone(struct mpuirq_data ** data); - -#ifdef __cplusplus -} -#endif - -#endif // HELPER_C_H diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.c b/libsensors_iio/software/simple_apps/common/mlerrorcode.c deleted file mode 100644 index 25b0df6..0000000 --- a/libsensors_iio/software/simple_apps/common/mlerrorcode.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/****************************************************************************** - * - * $Id: mlerrorcode.c 5629 2011-06-11 03:13:08Z mcaramello $ - * - *****************************************************************************/ - -#include -#include - -#include "mltypes.h" -#include "mlerrorcode.h" - -#define ERROR_CODE_CASE(CODE) \ - case CODE: \ - return #CODE \ - -/** - * @brief return a string containing the label assigned to the error code. - * - * @param errorcode - * The errorcode value of which the label has to be returned. - * - * @return A string containing the error code label. - */ -char* MLErrorCode(inv_error_t errorcode) -{ - switch(errorcode) { - ERROR_CODE_CASE(INV_SUCCESS); - ERROR_CODE_CASE(INV_ERROR); - ERROR_CODE_CASE(INV_ERROR_INVALID_PARAMETER); - ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_ENABLED); - ERROR_CODE_CASE(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - ERROR_CODE_CASE(INV_ERROR_DMP_NOT_STARTED); - ERROR_CODE_CASE(INV_ERROR_DMP_STARTED); - ERROR_CODE_CASE(INV_ERROR_NOT_OPENED); - ERROR_CODE_CASE(INV_ERROR_OPENED); - ERROR_CODE_CASE(INV_ERROR_INVALID_MODULE); - ERROR_CODE_CASE(INV_ERROR_MEMORY_EXAUSTED); - ERROR_CODE_CASE(INV_ERROR_DIVIDE_BY_ZERO); - ERROR_CODE_CASE(INV_ERROR_ASSERTION_FAILURE); - ERROR_CODE_CASE(INV_ERROR_FILE_OPEN); - ERROR_CODE_CASE(INV_ERROR_FILE_READ); - ERROR_CODE_CASE(INV_ERROR_FILE_WRITE); - - ERROR_CODE_CASE(INV_ERROR_SERIAL_CLOSED); - ERROR_CODE_CASE(INV_ERROR_SERIAL_OPEN_ERROR); - ERROR_CODE_CASE(INV_ERROR_SERIAL_READ); - ERROR_CODE_CASE(INV_ERROR_SERIAL_WRITE); - ERROR_CODE_CASE(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED); - - ERROR_CODE_CASE(INV_ERROR_SM_TRANSITION); - ERROR_CODE_CASE(INV_ERROR_SM_IMPROPER_STATE); - - ERROR_CODE_CASE(INV_ERROR_FIFO_OVERFLOW); - ERROR_CODE_CASE(INV_ERROR_FIFO_FOOTER); - ERROR_CODE_CASE(INV_ERROR_FIFO_READ_COUNT); - ERROR_CODE_CASE(INV_ERROR_FIFO_READ_DATA); - ERROR_CODE_CASE(INV_ERROR_MEMORY_SET); - - ERROR_CODE_CASE(INV_ERROR_LOG_MEMORY_ERROR); - ERROR_CODE_CASE(INV_ERROR_LOG_OUTPUT_ERROR); - - ERROR_CODE_CASE(INV_ERROR_OS_BAD_PTR); - ERROR_CODE_CASE(INV_ERROR_OS_BAD_HANDLE); - ERROR_CODE_CASE(INV_ERROR_OS_CREATE_FAILED); - ERROR_CODE_CASE(INV_ERROR_OS_LOCK_FAILED); - - ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_OVERFLOW); - ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_UNDERFLOW); - ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_NOT_READY); - ERROR_CODE_CASE(INV_ERROR_COMPASS_DATA_ERROR); - - ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LOAD); - ERROR_CODE_CASE(INV_ERROR_CALIBRATION_STORE); - ERROR_CODE_CASE(INV_ERROR_CALIBRATION_LEN); - ERROR_CODE_CASE(INV_ERROR_CALIBRATION_CHECKSUM); - - default: - { - #define UNKNOWN_ERROR_CODE 1234 - return ERROR_NAME(UNKNOWN_ERROR_CODE); - break; - } - - } -} - -/** - * @} - */ diff --git a/libsensors_iio/software/simple_apps/common/mlerrorcode.h b/libsensors_iio/software/simple_apps/common/mlerrorcode.h deleted file mode 100644 index 9a35792..0000000 --- a/libsensors_iio/software/simple_apps/common/mlerrorcode.h +++ /dev/null @@ -1,86 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/******************************************************************************* - * - * $Id: mltypes.h 3680 2010-09-04 03:13:32Z mcaramello $ - * - *******************************************************************************/ - -#ifndef _MLERRORCODE_H_ -#define _MLERRORCODE_H_ - -#include "mltypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* - Defines -*/ -#define CALL_N_CHECK(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - } \ -} - -#define CALL_CHECK_N_RETURN_ERROR(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - return r35uLt; \ - } \ -} - -// for functions returning void -#define CALL_CHECK_N_RETURN(f) do { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - return; \ - } \ - } while(0) - -#define CALL_CHECK_N_EXIT(f) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - exit (r35uLt); \ - } \ -} - - -#define CALL_CHECK_N_CALLBACK(f, cb) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - cb; \ - } \ -} - -#define CALL_CHECK_N_GOTO(f, label) { \ - unsigned int r35uLt = f; \ - if(INV_SUCCESS != r35uLt) { \ - MPL_LOGE("Error in file %s, line %d : %s returned code %s (#%d)\n", \ - __FILE__, __LINE__, #f, MLErrorCode(r35uLt), r35uLt); \ - goto label; \ - } \ -} - -char* MLErrorCode(inv_error_t errorcode); - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.c b/libsensors_iio/software/simple_apps/common/mlsetup.c deleted file mode 100644 index f11bce9..0000000 --- a/libsensors_iio/software/simple_apps/common/mlsetup.c +++ /dev/null @@ -1,1722 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/****************************************************************************** - * - * $Id: mlsetup.c 6113 2011-09-29 23:40:55Z jcalizo $ - * - *****************************************************************************/ -#undef MPL_LOG_NDEBUG -#ifdef UNITTESTING -#define MPL_LOG_NDEBUG 1 -#else -#define MPL_LOG_NDEBUG 0 -#endif - -/** - * @defgroup MLSETUP - * @brief The Motion Library external slaves setup override suite. - * - * Use these APIs to override the kernel/default settings in the - * corresponding data structures for gyros, accel, and compass. - * - * @{ - * @file mlsetup.c - * @brief The Motion Library external slaves setup override suite. - */ - -/* ------------------ */ -/* - Include Files. - */ -/* ------------------ */ - -/* - Defines -*/ -/* these have to appear before inclusion of mpu.h */ -#define CONFIG_MPU_SENSORS_KXSD9 y // Kionix accel -#define CONFIG_MPU_SENSORS_KXTF9 y // Kionix accel -#define CONFIG_MPU_SENSORS_LIS331DLH y // ST accelerometer -#define CONFIG_MPU_SENSORS_LSM303DLX_A y // ST accelerometer in LSM303DLx combo -#define CONFIG_MPU_SENSORS_LIS3DH y // ST accelerometer -#define CONFIG_MPU_SENSORS_BMA150 y // Bosch 150 accelerometer -#define CONFIG_MPU_SENSORS_BMA222 y // Bosch 222 accelerometer -#define CONFIG_MPU_SENSORS_BMA250 y // Bosch 250 accelerometer -#define CONFIG_MPU_SENSORS_ADXL34X y // AD 345 or 346 accelerometer -#define CONFIG_MPU_SENSORS_MMA8450 y // Freescale MMA8450 accelerometer -#define CONFIG_MPU_SENSORS_MMA845X y // Freescale MMA845X accelerometer -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 -#define CONFIG_MPU_SENSORS_MPU6050_ACCEL y // Invensense MPU6050 built-in accelerometer -#endif - -#define CONFIG_MPU_SENSORS_AK8975 y // AKM compass -#define CONFIG_MPU_SENSORS_AMI30X y // AICHI AMI304/305 compass -#define CONFIG_MPU_SENSORS_AMI306 y // AICHI AMI306 compass -#define CONFIG_MPU_SENSORS_HMC5883 y // Honeywell compass -#define CONFIG_MPU_SENSORS_LSM303DLX_M y // ST compass in LSM303DLx combo -#define CONFIG_MPU_SENSORS_YAS529 y // Yamaha compass -#define CONFIG_MPU_SENSORS_YAS530 y // Yamaha compass -#define CONFIG_MPU_SENSORS_MMC314X y // MEMSIC compass -#define CONFIG_MPU_SENSORS_HSCDTD002B y // ALPS compass -#define CONFIG_MPU_SENSORS_HSCDTD004A y // ALPS HSCDTD004A compass - -#define CONFIG_MPU_SENSORS_BMA085 y // Bosch 085 pressure - -#include "external_hardware.h" - -#include -#include - -#include "slave.h" -#include "compass.h" -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "MPL-mlsetup" - -#include "linux/mpu.h" - -#include "mlsetup.h" - -#ifdef LINUX -#include "errno.h" -#endif - -/* Override these structures from mldl.c */ -extern struct ext_slave_descr g_slave_accel; -extern struct ext_slave_descr g_slave_compass; -//extern struct ext_slave_descr g_slave_pressure; -/* Platform Data */ -//extern struct mpu_platform_data g_pdata; -extern struct ext_slave_platform_data g_pdata_slave_accel; -extern struct ext_slave_platform_data g_pdata_slave_compass; -//extern struct ext_slave_platform_data g_pdata_slave_pressure; -signed char g_gyro_orientation[9]; - -/* - Typedefs -*/ -typedef void tSetupFuncAccel(void); -typedef void tSetupFuncCompass(void); -typedef void tSetupFuncPressure(void); - -#ifdef LINUX -#include -#endif - -/********************************************************************* - Dragon - PLATFORM_ID_DRAGON_PROTOTYPE -*********************************************************************/ -/** - * @internal - * @brief performs a 180' rotation around Z axis to reflect - * usage of the multi sensor board (MSB) with the - * beagleboard - * @note assumes well formed mounting matrix, with only - * one 1 for each row. - */ -static void Rotate180DegAroundZAxis(signed char matrix[]) -{ - int ii; - for(ii=0; ii<6; ii++) { - matrix[ii] = -matrix[ii]; - } -} - -/** - * @internal - * Sets the orientation based on the position of the mounting. For different - * devices the relative position to pin 1 will be different. - * - * Positions are: - * - 0-3 are Z up - * - 4-7 are Z down - * - 8-11 are Z right - * - 12-15 are Z left - * - 16-19 are Z front - * - 20-23 are Z back - * - * @param position The position of the orientation - * @param orientation the location to store the new oreintation - */ -static inv_error_t SetupOrientation(unsigned int position, - signed char *orientation) -{ - memset(orientation, 0, 9); - switch (position){ - case 0: - /*-------------------------*/ - orientation[0] = +1; - orientation[4] = +1; - orientation[8] = +1; - break; - case 1: - /*-------------------------*/ - orientation[1] = +1; - orientation[3] = -1; - orientation[8] = +1; - break; - case 2: - /*-------------------------*/ - orientation[0] = -1; - orientation[4] = -1; - orientation[8] = +1; - break; - case 3: - /*-------------------------*/ - orientation[1] = -1; - orientation[3] = +1; - orientation[8] = +1; - break; - case 4: - /*-------------------------*/ - orientation[0] = -1; - orientation[4] = +1; - orientation[8] = -1; - break; - case 5: - /*-------------------------*/ - orientation[1] = -1; - orientation[3] = -1; - orientation[8] = -1; - break; - case 6: - /*-------------------------*/ - orientation[0] = +1; - orientation[4] = -1; - orientation[8] = -1; - break; - case 7: - /*-------------------------*/ - orientation[1] = +1; - orientation[3] = +1; - orientation[8] = -1; - break; - case 8: - /*-------------------------*/ - orientation[2] = +1; - orientation[3] = +1; - orientation[7] = +1; - break; - case 9: - /*-------------------------*/ - orientation[2] = +1; - orientation[4] = +1; - orientation[6] = -1; - break; - case 10: - orientation[2] = +1; - orientation[3] = -1; - orientation[7] = -1; - break; - case 11: - orientation[2] = +1; - orientation[4] = -1; - orientation[6] = +1; - break; - case 12: - orientation[2] = -1; - orientation[3] = -1; - orientation[7] = +1; - break; - case 13: - orientation[2] = -1; - orientation[4] = -1; - orientation[6] = -1; - break; - case 14: - orientation[2] = -1; - orientation[3] = +1; - orientation[7] = -1; - break; - case 15: - orientation[2] = -1; - orientation[4] = +1; - orientation[6] = +1; - break; - case 16: - orientation[0] = -1; - orientation[5] = +1; - orientation[7] = +1; - break; - case 17: - orientation[1] = -1; - orientation[5] = +1; - orientation[6] = -1; - break; - case 18: - orientation[0] = +1; - orientation[5] = -1; - orientation[7] = -1; - break; - case 19: - orientation[1] = -1; - orientation[5] = +1; - orientation[6] = +1; - break; - case 20: - orientation[0] = +1; - orientation[5] = -1; - orientation[7] = +1; - break; - case 21: - orientation[1] = -1; - orientation[5] = -1; - orientation[6] = +1; - break; - case 22: - orientation[0] = -1; - orientation[5] = -1; - orientation[7] = -1; - break; - case 23: - orientation[1] = +1; - orientation[5] = -1; - orientation[6] = -1; - break; - default: - MPL_LOGE("Invalid position %d\n", position); - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - return INV_SUCCESS; -} - -static void PrintMountingOrientation( - const char * header, signed char *orientation) -{ - MPL_LOGV("%s:\n", header); - MPL_LOGV("\t[[%3d, %3d, %3d]\n", - orientation[0], orientation[1], orientation[2]); - MPL_LOGV("\t [%3d, %3d, %3d]\n", - orientation[3], orientation[4], orientation[5]); - MPL_LOGV("\t [%3d, %3d, %3d]]\n", - orientation[6], orientation[7], orientation[8]); -} - -/***************************** - * Accel Setup Functions * - *****************************/ - -static inv_error_t SetupAccelSTLIS331Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_SPIDER_PROTOTYPE: - position = 5; - break; - case PLATFORM_ID_ST_6AXIS: - position = 0; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *lis331_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS331; - return INV_SUCCESS; -} - -static inv_error_t SetupAccelSTLIS3DHCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_SPIDER_PROTOTYPE: - position = 1; - break; - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - position = 3; - break; - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *lis3dh_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LIS3DH; - return result; -} - -static inv_error_t SetupAccelKionixKXSD9Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 7; - break; - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *kxsd9_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXSD9; - return result; -} - -static inv_error_t SetupAccelKionixKXTF9Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB_EVB: - position =0; - break; - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_SPIDER_PROTOTYPE: - position = 7; - break; -#ifdef WIN32 - case PLATFORM_ID_DONGLE: - position = 1; - break; -#endif - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - position = 1; - break; - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *kxtf9_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_KXTF9; - return result; -} - -static inv_error_t SetupAccelLSM303Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - position = 3; - break; - case PLATFORM_ID_MSB_V2: - position = 1; - break; - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *lsm303dlx_a_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_LSM303; - return result; -} - -static inv_error_t SetupAccelBMA150Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 6; - break; -#ifdef WIN32 - case PLATFORM_ID_DONGLE: - position = 3; - break; -#endif - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *bma150_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA150; - return result; -} - -static inv_error_t SetupAccelBMA222Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 0; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *bma222_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA222; - return result; -} - -static inv_error_t SetupAccelBMA250Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - position = 0; - break; - case PLATFORM_ID_SPIDER_PROTOTYPE: - position = 3; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *bma250_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_BMA250; - return result; -} - -static inv_error_t SetupAccelADXL34XCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 6; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *adxl34x_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_ADXL34X; - return result; -} - - -static inv_error_t SetupAccelMMA8450Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 5; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *mma8450_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA8450; - return result; -} - - -static inv_error_t SetupAccelMMA845XCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 5; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_accel.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_accel = *mma845x_get_slave_descr(); -#endif - g_pdata_slave_accel.address = ACCEL_SLAVEADDR_MMA845X; - return result; -} - - -/** - * @internal - * Sets up the orientation matrix according to how the gyro was - * mounted. - * - * @param platforId Platform identification for mounting information - * @return INV_SUCCESS or non-zero error code - */ -static inv_error_t SetupAccelMPU6050Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_MANTIS_MSB: - position = 6; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_DRAGON_USB_DONGLE: - position = 1; - break; - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - case PLATFORM_ID_MANTIS_EVB: - position = 0; - break; - case PLATFORM_ID_MSB_V3: - position = 2; - break; - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - return INV_ERROR_INVALID_PARAMETER; - }; - - SetupOrientation(position, g_pdata_slave_accel.orientation); - /* Interrupt */ -#ifndef LINUX -#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 - // g_slave_accel = // fixme *mpu6050_get_slave_descr(); -#endif -#endif - g_pdata_slave_accel.address = 0x68; - return result; -} - -/***************************** - Compass Setup Functions -******************************/ -static inv_error_t SetupCompassAKM8975Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_MANTIS_MSB: - position = 2; - break; -#ifdef WIN32 - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_MANTIS_USB_DONGLE: - position = 4; - break; -#endif - case PLATFORM_ID_SPIDER_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - position = 7; - break; - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V3: - case PLATFORM_ID_MSB_V2_MANTIS: - position = 6; - break; - case PLATFORM_ID_DRAGON_USB_DONGLE: - case PLATFORM_ID_MSB_EVB: - position = 5; - break; - case PLATFORM_ID_MANTIS_EVB: - position = 4; - break; - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *ak8975_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AKM; - return result; -} - -static inv_error_t SetupCompassMMCCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 7; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *mmc314x_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_MMC314X; - return result; -} - -static inv_error_t SetupCompassAMI304Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 4; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI304; -#ifndef LINUX - g_slave_compass = *ami30x_get_slave_descr(); -#endif - return result; -} - -static inv_error_t SetupCompassAMI306Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_SPIDER_PROTOTYPE: - position = 3; - break; - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - position = 1; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *ami306_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_AMI306; - return result; -} - -static inv_error_t SetupCompassHMC5883Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 6; - break; -#ifdef WIN32 - case PLATFORM_ID_DONGLE: - position = 2; - break; -#endif - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *hmc5883_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; - return result; -} - - -static inv_error_t SetupCompassLSM303DLHCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_10AXIS: - position = 1; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - }; - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } -#ifndef LINUX - g_slave_compass = *lsm303dlx_m_get_slave_descr(); - g_slave_compass.id = COMPASS_ID_LSM303DLH; -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; - return result; -} - -static inv_error_t SetupCompassLSM303DLMCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - position = 8; - break; - case PLATFORM_ID_MSB_V2: - position = 12; - break; - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - }; - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *lsm303dlx_m_get_slave_descr(); - g_slave_compass.id = COMPASS_ID_LSM303DLM; -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HMC5883; - return result; -} - -static inv_error_t SetupCompassYAS530Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - position = 1; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *yas530_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS530; - return result; -} - -static inv_error_t SetupCompassYAS529Calibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 6; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *yas529_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_YAS529; - return result; -} - - -static inv_error_t SetupCompassHSCDTD002BCalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 2; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *hscdtd002b_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX; - return result; -} - -static inv_error_t SetupCompassHSCDTD004ACalibration(unsigned short platformId) -{ - inv_error_t result = INV_SUCCESS; - unsigned int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - position = 1; - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_MANTIS_MSB: - case PLATFORM_ID_MANTIS_USB_DONGLE: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); - return INV_ERROR_FEATURE_NOT_IMPLEMENTED; - }; - result = SetupOrientation(position, g_pdata_slave_compass.orientation); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - -#ifndef LINUX - g_slave_compass = *hscdtd004a_get_slave_descr(); -#endif - g_pdata_slave_compass.address = COMPASS_SLAVEADDR_HSCDTD00XX; - return result; -} - - -/***************************** - Pressure Setup Functions -******************************/ -#if 0 -static inv_error_t SetupPressureBMA085Calibration(unsigned short platformId) -{ - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - memset(g_pdata_slave_pressure.orientation, 0, sizeof(g_pdata_slave_pressure.orientation)); - - g_pdata_slave_pressure.bus = EXT_SLAVE_BUS_PRIMARY; -#ifndef LINUX - g_slave_pressure = *bma085_get_slave_descr(); -#endif - g_pdata_slave_pressure.address = PRESSURE_SLAVEADDR_BMA085; - return INV_SUCCESS; -} -#endif -/** - * @internal - * Sets up the orientation matrix according to how the part was - * mounted. - * - * @param platforId Platform identification for mounting information - * @return INV_SUCCESS or non-zero error code - */ -static inv_error_t SetupAccelCalibration(unsigned short platformId, - unsigned short accelId) -{ - /*---- setup the accels ----*/ - switch(accelId) { - case ACCEL_ID_LSM303DLX: - SetupAccelLSM303Calibration(platformId); - break; - case ACCEL_ID_LIS331: - SetupAccelSTLIS331Calibration(platformId); - break; - case ACCEL_ID_KXSD9: - SetupAccelKionixKXSD9Calibration(platformId); - break; - case ACCEL_ID_KXTF9: - SetupAccelKionixKXTF9Calibration(platformId); - break; - case ACCEL_ID_BMA150: - SetupAccelBMA150Calibration(platformId); - break; - case ACCEL_ID_BMA222: - SetupAccelBMA222Calibration(platformId); - break; - case ACCEL_ID_BMA250: - SetupAccelBMA250Calibration(platformId); - break; - case ACCEL_ID_ADXL34X: - SetupAccelADXL34XCalibration(platformId); - break; - case ACCEL_ID_MMA8450: - SetupAccelMMA8450Calibration(platformId); - break; - case ACCEL_ID_MMA845X: - SetupAccelMMA845XCalibration(platformId); - break; - case ACCEL_ID_LIS3DH: - SetupAccelSTLIS3DHCalibration(platformId); - break; - case ACCEL_ID_MPU6050: - SetupAccelMPU6050Calibration(platformId); - break; - case ID_INVALID: - break; - default: - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - - if (accelId != ID_INVALID && accelId != ACCEL_ID_MPU6050) { - g_pdata_slave_accel.bus = EXT_SLAVE_BUS_SECONDARY; - } else if (accelId != ACCEL_ID_MPU6050) { - g_pdata_slave_accel.bus = EXT_SLAVE_BUS_PRIMARY; - } - -#ifndef WIN32 - if (accelId != ID_INVALID) - Rotate180DegAroundZAxis(g_pdata_slave_accel.orientation); -#endif - - return INV_SUCCESS; -} - -/** - * @internal - * Sets up the orientation matrix according to how the part was - * mounted. - * - * @param platforId Platform identification for mounting information - * @return INV_SUCCESS or non-zero error code - */ -inv_error_t SetupCompassCalibration(unsigned short platformId, - unsigned short compassId) -{ - /*---- setup the compass ----*/ - switch(compassId) { - case COMPASS_ID_AK8975: - SetupCompassAKM8975Calibration(platformId); - break; - case COMPASS_ID_AMI30X: - SetupCompassAMI304Calibration(platformId); - break; - case COMPASS_ID_AMI306: - SetupCompassAMI306Calibration(platformId); - break; - case COMPASS_ID_LSM303DLH: - SetupCompassLSM303DLHCalibration(platformId); - break; - case COMPASS_ID_LSM303DLM: - SetupCompassLSM303DLMCalibration(platformId); - break; - case COMPASS_ID_HMC5883: - SetupCompassHMC5883Calibration(platformId); - break; - case COMPASS_ID_YAS529: - SetupCompassYAS529Calibration(platformId); - break; - case COMPASS_ID_YAS530: - SetupCompassYAS530Calibration(platformId); - break; - case COMPASS_ID_MMC314X: - SetupCompassMMCCalibration(platformId); - break; - case COMPASS_ID_HSCDTD002B: - SetupCompassHSCDTD002BCalibration(platformId); - break; - case COMPASS_ID_HSCDTD004A: - SetupCompassHSCDTD004ACalibration(platformId); - break; - case ID_INVALID: - break; - default: - if (INV_ERROR_INVALID_PARAMETER) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - break; - } - - if (platformId == PLATFORM_ID_MSB_V2_MANTIS || - platformId == PLATFORM_ID_MANTIS_MSB || - platformId == PLATFORM_ID_MANTIS_USB_DONGLE || - platformId == PLATFORM_ID_MANTIS_PROTOTYPE || - platformId == PLATFORM_ID_DRAGON_PROTOTYPE) { - switch (compassId) { - case ID_INVALID: - g_pdata_slave_compass.bus = EXT_SLAVE_BUS_INVALID; - break; - case COMPASS_ID_AK8975: - case COMPASS_ID_AMI306: - g_pdata_slave_compass.bus = EXT_SLAVE_BUS_SECONDARY; - break; - default: - g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY; - }; - } else { - g_pdata_slave_compass.bus = EXT_SLAVE_BUS_PRIMARY; - } - -#ifndef WIN32 - if (compassId != ID_INVALID) - Rotate180DegAroundZAxis(g_pdata_slave_compass.orientation); -#endif - - return INV_SUCCESS; -} - -/** - * @internal - * Sets up the orientation matrix according to how the part was - * mounted. - * - * @param platforId Platform identification for mounting information - * @return INV_SUCCESS or non-zero error code - */ -#if 0 -inv_error_t SetupPressureCalibration(unsigned short platformId, - unsigned short pressureId) -{ - inv_error_t result = INV_SUCCESS; - /*---- setup the compass ----*/ - switch(pressureId) { - case PRESSURE_ID_BMA085: - result = SetupPressureBMA085Calibration(platformId); - break; - default: - if (INV_ERROR_INVALID_PARAMETER) { - LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); - return INV_ERROR_INVALID_PARAMETER; - } - }; - - return result; -} -#endif -/** - * @internal - * Sets up the orientation matrix according to how the gyro was - * mounted. - * - * @param platforId Platform identification for mounting information - * @return INV_SUCCESS or non-zero error code - */ -static inv_error_t SetupGyroCalibration(unsigned short platformId) -{ - int position; - MPL_LOGV("Calibrating '%s'\n", __func__); - - /* Orientation */ - switch (platformId) { - case PLATFORM_ID_SPIDER_PROTOTYPE: - position = 2; - break; - case PLATFORM_ID_MSB: - case PLATFORM_ID_MSB_10AXIS: - case PLATFORM_ID_MANTIS_MSB: -#ifndef WIN32 - position = 4; -#else - position = 6; -#endif - break; - case PLATFORM_ID_DONGLE: - case PLATFORM_ID_MANTIS_USB_DONGLE: - position = 1; - break; - case PLATFORM_ID_DRAGON_USB_DONGLE: - position = 3; - break; - case PLATFORM_ID_MANTIS_PROTOTYPE: - case PLATFORM_ID_DRAGON_PROTOTYPE: - case PLATFORM_ID_ST_6AXIS: - case PLATFORM_ID_MSB_V2: - case PLATFORM_ID_MSB_V2_MANTIS: -#ifndef WIN32 - position = 2; -#else - position = 0; -#endif - break; - case PLATFORM_ID_MANTIS_EVB: - case PLATFORM_ID_MSB_EVB: - position = 0; - break; - case PLATFORM_ID_MSB_V3: - position = 2; - break; - default: - MPL_LOGE("Unsupported platform %d\n", platformId); - return INV_ERROR_INVALID_PARAMETER; - }; - - SetupOrientation(position, g_gyro_orientation); - - return INV_SUCCESS; -} - -/** - * @brief Setup the Hw orientation and full scale. - * @param platfromId - * an user defined Id to distinguish the Hw platform in - * use from others. - * @param accelId - * the accelerometer specific id, as specified in the MPL. - * @param compassId - * the compass specific id, as specified in the MPL. - * @return INV_SUCCESS or a non-zero error code. - */ -inv_error_t SetupPlatform( - unsigned short platformId, - unsigned short accelId, - unsigned short compassId) -{ - int result; - - memset(&g_slave_accel, 0, sizeof(g_slave_accel)); - memset(&g_slave_compass, 0, sizeof(g_slave_compass)); -// memset(&g_slave_pressure, 0, sizeof(g_slave_pressure)); -// memset(&g_pdata, 0, sizeof(g_pdata)); - -#ifdef LINUX - /* On Linux initialize the global platform data with the driver defaults */ - { - void *mpu_handle; - int ii; - - struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES]; - struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES]; - slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; - slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel; - slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass; - //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure; - - pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; - pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel; - pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass; - //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure; - - MPL_LOGI("Getting the MPU_GET_PLATFORM_DATA\n"); - result = inv_serial_open("/dev/mpu",&mpu_handle); - if (result) { - MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result); - } - for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { - if (!slave[ii]) - continue; - slave[ii]->type = ii; - result = ioctl((int)mpu_handle, MPU_GET_EXT_SLAVE_DESCR, - slave[ii]); - if (result) - result = errno; - if(result == INV_ERROR_INVALID_MODULE) { - slave[ii] = NULL; - result = 0; - } else if (result) { - LOG_RESULT_LOCATION(result); - LOG_RESULT_LOCATION(INV_ERROR_INVALID_MODULE); - return result; - } - } - //result = ioctl((int)mpu_handle, MPU_GET_MPU_PLATFORM_DATA, &g_pdata); - if (result) { - result = errno; - LOG_RESULT_LOCATION(result); - return result; - } - for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { - if (!pdata_slave[ii]) - continue; - pdata_slave[ii]->type = ii; - result = ioctl( - (int)mpu_handle, MPU_GET_EXT_SLAVE_PLATFORM_DATA, - pdata_slave[ii]); - if (result) - result = errno; - if (result == INV_ERROR_INVALID_MODULE) { - pdata_slave[ii] = NULL; - result = 0; - } else if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - if (result) { - MPL_LOGE("MPU_GET_PLATFORM_DATA failed %d\n", result); - } - inv_serial_close(mpu_handle); - } -#endif - - result = SetupGyroCalibration(platformId); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - PrintMountingOrientation("Gyroscope", g_gyro_orientation); - result = SetupAccelCalibration(platformId, accelId); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - PrintMountingOrientation("Accelerometer", g_pdata_slave_accel.orientation); - result = SetupCompassCalibration(platformId, compassId); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - PrintMountingOrientation("Compass", g_pdata_slave_compass.orientation); -#if 0 - if (platformId == PLATFORM_ID_MSB_10AXIS) { - result = SetupPressureCalibration(platformId, PRESSURE_ID_BMA085); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - PrintMountingOrientation("Pressure", g_pdata_slave_pressure.orientation); - } -#endif -#ifdef LINUX - /* On Linux override the orientation, level shifter etc */ - { - void *mpu_handle; - int ii; - struct ext_slave_descr *slave[EXT_SLAVE_NUM_TYPES]; - struct ext_slave_platform_data *pdata_slave[EXT_SLAVE_NUM_TYPES]; - slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; - slave[EXT_SLAVE_TYPE_ACCEL] = &g_slave_accel; - slave[EXT_SLAVE_TYPE_COMPASS] = &g_slave_compass; - //slave[EXT_SLAVE_TYPE_PRESSURE] = &g_slave_pressure; - - pdata_slave[EXT_SLAVE_TYPE_GYROSCOPE] = NULL; - pdata_slave[EXT_SLAVE_TYPE_ACCEL] = &g_pdata_slave_accel; - pdata_slave[EXT_SLAVE_TYPE_COMPASS] = &g_pdata_slave_compass; - //pdata_slave[EXT_SLAVE_TYPE_PRESSURE] = &g_pdata_slave_pressure; - - MPL_LOGI("Setting the MPU_SET_PLATFORM_DATA\n"); - result = inv_serial_open("/dev/mpu",&mpu_handle); - if (result) { - MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result); - } - for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { - if (!slave[ii]) - continue; - slave[ii]->type = ii; - result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA, - slave[ii]); - if (result) - result = errno; - if (result == INV_ERROR_INVALID_MODULE) { - slave[ii] = NULL; - result = 0; - } else if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - //result = ioctl((int)mpu_handle, MPU_SET_MPU_PLATFORM_DATA, &g_pdata); - if (result) { - result = errno; - LOG_RESULT_LOCATION(result); - return result; - } - for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { - if (!pdata_slave[ii]) - continue; - pdata_slave[ii]->type = ii; - result = ioctl((int)mpu_handle, MPU_SET_EXT_SLAVE_PLATFORM_DATA, - pdata_slave[ii]); - if (result) - result = errno; - if (result == INV_ERROR_INVALID_MODULE) { - pdata_slave[ii] = NULL; - result = 0; - } else if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - } - if (result) { - MPL_LOGE("MPU_SET_PLATFORM_DATA failed %d\n", result); - } - inv_serial_close(mpu_handle); - } -#endif - return INV_SUCCESS; -} - -/** - * @} - */ - - diff --git a/libsensors_iio/software/simple_apps/common/mlsetup.h b/libsensors_iio/software/simple_apps/common/mlsetup.h deleted file mode 100644 index 06fa9f4..0000000 --- a/libsensors_iio/software/simple_apps/common/mlsetup.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ - -/******************************************************************************* - * - * $Id: mlsetup.h 6101 2011-09-29 00:30:33Z kkatingari $ - * - *******************************************************************************/ - -#ifndef MLSETUP_H -#define MLSETUP_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "linux/mpu.h" -#include "mltypes.h" - - enum mpu_platform_id { - PLATFORM_ID_INVALID = ID_INVALID, // 0 - PLATFORM_ID_MSB, // (0x0001) MSB (Multi sensors board) - PLATFORM_ID_ST_6AXIS, // (0x0002) 6 Axis with ST accelerometer - PLATFORM_ID_DONGLE, // (0x0003) 9 Axis USB dongle with - PLATFORM_ID_MANTIS_PROTOTYPE, // (0x0004) Mantis prototype board - PLATFORM_ID_MANTIS_MSB, // (0x0005) MSB with Mantis - PLATFORM_ID_MANTIS_USB_DONGLE,// (0x0006) Mantis and AKM on USB dongle. - PLATFORM_ID_MSB_10AXIS, // (0x0007) MSB with pressure sensor - PLATFORM_ID_DRAGON_PROTOTYPE, // (0x0008) Dragon prototype board - PLATFORM_ID_MSB_V2, // (0x0009) Version 2 MSB - PLATFORM_ID_MSB_V2_MANTIS, // (0x000A) Version 2 MSB with mantis - PLATFORM_ID_MANTIS_EVB, // (0x000B) Mantis EVB (shipped to cust.) - PLATFORM_ID_DRAGON_USB_DONGLE,// (0x000C) Dragon USB Dongle with Mantis Rev C - PLATFORM_ID_MSB_EVB, // (0X000D) MSB with 3050. - PLATFORM_ID_SPIDER_PROTOTYPE, - PLATFORM_ID_MSB_V3, - - NUM_PLATFORM_IDS - }; - // Main entry APIs -inv_error_t SetupPlatform(unsigned short platformId, - unsigned short accelSelection, - unsigned short compassSelection); - -#ifdef __cplusplus -} -#endif - -#endif /* MLSETUP_H */ diff --git a/libsensors_iio/software/simple_apps/common/slave.h b/libsensors_iio/software/simple_apps/common/slave.h deleted file mode 100644 index 7b40a8c..0000000 --- a/libsensors_iio/software/simple_apps/common/slave.h +++ /dev/null @@ -1,176 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -/******************************************************************************* - * - * $Id: slave.h 5732 2011-07-07 01:11:34Z vbhatt $ - * - *******************************************************************************/ - -#ifndef SLAVE_H -#define SLAVE_H - -/** - * @addtogroup SLAVEDL - * - * @{ - * @file slave.h - * @brief Top level descriptions for Accelerometer support - * - */ - -#include "mltypes.h" -#include "linux/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_LSM303DLX -#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/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared b/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared deleted file mode 100644 index 228abf7..0000000 Binary files a/libsensors_iio/software/simple_apps/console/linux/build/android/consoledmp-shared and /dev/null differ diff --git a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk b/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk deleted file mode 100644 index b1d881c..0000000 --- a/libsensors_iio/software/simple_apps/console/linux/build/android/shared.mk +++ /dev/null @@ -1,109 +0,0 @@ -EXEC = consoledmp$(SHARED_APP_SUFFIX) - -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 = ../../../../../.. -APP_DIR = $(CURDIR)/../.. -MLLITE_DIR = $(INV_ROOT)/software/core/mllite -COMMON_DIR = $(INV_ROOT)/software/simple_apps/common -MPL_DIR = $(INV_ROOT)/software/core/mpl -HAL_DIR = $(INV_ROOT)/software/core/HAL - -include $(INV_ROOT)/software/build/android/common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -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$(MPL_DIR) -CFLAGS += -I$(COMMON_DIR) -CFLAGS += -I$(HAL_DIR)/include -CFLAGS += $(INV_INCLUDES) -CFLAGS += $(INV_DEFINES) - -LLINK = -lc -LLINK += -lm -LLINK += -lutils -LLINK += -lcutils -LLINK += -lgcc -LLINK += -ldl -LLINK += -lstdc++ -LLINK += -llog -LLINK += -lz - -PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x -PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o -PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o - -LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += -nostdlib -LFLAGS += -fpic -LFLAGS += -Wl,--gc-sections -LFLAGS += -Wl,--no-whole-archive -LFLAGS += -Wl,-dynamic-linker,/system/bin/linker -LFLAGS += $(ANDROID_LINK) -ifneq ($(PRODUCT),panda) -LFLAGS += -rdynamic -endif - -LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib - -#################################################################################################### -## sources - -INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) -INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) - -#INV_SOURCES and 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 clean cleanall install - -all: $(EXEC) $(MK_NAME) - -$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n\n") - mkdir obj - -$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n\n") - $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(EXEC) $(OBJFOLDER) - -install : $(EXEC) - cp -f $(EXEC) $(INSTALL_DIR) - - diff --git a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk b/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk deleted file mode 100644 index b01fdfb..0000000 --- a/libsensors_iio/software/simple_apps/console/linux/build/filelist.mk +++ /dev/null @@ -1,23 +0,0 @@ -#### filelist.mk for console_test #### - -# helper headers -HEADERS := $(COMMON_DIR)/external_hardware.h -HEADERS += $(COMMON_DIR)/fopenCMake.h -HEADERS += $(COMMON_DIR)/helper.h -HEADERS += $(COMMON_DIR)/mlerrorcode.h -HEADERS += $(COMMON_DIR)/mlsetup.h -HEADERS += $(COMMON_DIR)/slave.h - -HEADERS += $(HAL_DIR)/include/mlos.h -HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h - -# sources -SOURCES := $(APP_DIR)/console_test.c - -# helper sources -SOURCES += $(HAL_DIR)/linux/inv_sysfs_utils.c -SOURCES += $(HAL_DIR)/linux/mlos_linux.c - -INV_SOURCES += $(SOURCES) - -VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/libsensors_iio/software/simple_apps/console/linux/console_test.c b/libsensors_iio/software/simple_apps/console/linux/console_test.c deleted file mode 100644 index e15b20d..0000000 --- a/libsensors_iio/software/simple_apps/console/linux/console_test.c +++ /dev/null @@ -1,742 +0,0 @@ -/******************************************************************************* - * $Id: $ - ******************************************************************************/ - -/******************************************************************************* - * - * Copyright (c) 2011 InvenSense Corporation, All Rights Reserved. - * - ******************************************************************************/ -//#include -//#include -//#include -//#include - -//#if 0 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "log.h" -#undef MPL_LOG_TAG -#define MPL_LOG_TAG "console_test" - -#include "mlos.h" -#include "invensense.h" -#include "invensense_adv.h" -#include "inv_sysfs_utils.h" -#include "ml_stored_data.h" -#include "ml_math_func.h" -#include "ml_load_dmp.h" -/*#else -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "inv_sysfs_utils.h" -#include "invensense.h" -#include "invensense_adv.h" -#include "ml_stored_data.h" -#include "ml_math_func.h" -#include "ml_load_dmp.h" -#include "log.h" -#endif*/ - -/* TODO: add devices as needed. */ -#define ITG3500 (0) -#define MPU6050 (1) -#define BMA250 (2) -#define NUM_DEVICES (ITG3500 + MPU6050 + BMA250) - -#define DEVICE MPU6050 -#define DMP_IMAGE dmp_firmware_200_latest -#define SIX_AXES 6 -#define NINE_AXES 9 - -#if 0 -struct input_event { - struct timeval time; - __u16 type; - __u16 code; - __s32 value; -}; -#endif - -/* TODO: Add paths to other attributes. - * TODO: Input device paths depend on the module installation order. - */ -const struct inv_sysfs_names_s filenames[NUM_DEVICES] = { - { /* ITG3500 */ - .buffer = "/dev/input/event0", - .enable = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_enable", - .raw_data = "/sys/bus/i2c/devices/4-0068/inv_gyro/raw_gyro", - .temperature = "/sys/bus/i2c/devices/4-0068/inv_gyro/temperature", - .fifo_rate = "/sys/bus/i2c/devices/4-0068/inv_gyro/fifo_rate", - .power_state = "/sys/bus/i2c/devices/4-0068/inv_gyro/power_state", - .fsr = "/sys/bus/i2c/devices/4-0068/inv_gyro/FSR", - .lpf = "/sys/bus/i2c/devices/4-0068/inv_gyro/lpf", - .scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/gyro_scale", - .temp_scale = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_scale", - .temp_offset = "/sys/bus/i2c/devices/4-0068/inv_gyro/temp_offset", - .self_test = "/sys/bus/i2c/devices/4-0068/inv_gyro/self_test", - .accel_en = NULL, - .accel_fifo_en = NULL, - .accel_fs = NULL, - .clock_source = NULL, - .early_suspend_en = NULL, - .firmware_loaded = NULL, - .gyro_en = NULL, - .gyro_fifo_en = NULL, - .key = NULL, - .raw_accel = NULL, - .reg_dump = NULL, - .tap_on = NULL, - .dmp_firmware = NULL - }, - - { /* MPU6050 */ - .buffer = "/dev/input/event0", - .enable = "/sys/class/invensense/mpu/enable", - .raw_data = "/sys/class/invensense/mpu/raw_gyro", - .temperature = "/sys/class/invensense/mpu/temperature", - .fifo_rate = "/sys/class/invensense/mpu/fifo_rate", - .power_state = "/sys/class/invensense/mpu/power_state", - .fsr = "/sys/class/invensense/mpu/FSR", - .lpf = "/sys/class/invensense/mpu/lpf", - .scale = "/sys/class/invensense/mpu/gyro_scale", - .temp_scale = "/sys/class/invensense/mpu/temp_scale", - .temp_offset = "/sys/class/invensense/mpu/temp_offset", - .self_test = "/sys/class/invensense/mpu/self_test", - .accel_en = "/sys/class/invensense/mpu/accl_enable", - .accel_fifo_en = "/sys/class/invensense/mpu/accl_fifo_enable", - .accel_fs = "/sys/class/invensense/mpu/accl_fs", - .clock_source = "/sys/class/invensense/mpu/clock_source", - .early_suspend_en = "/sys/class/invensense/mpu/early_suspend_enable", - .firmware_loaded = "/sys/class/invensense/mpu/firmware_loaded", - .gyro_en = "/sys/class/invensense/mpu/gyro_enable", - .gyro_fifo_en = "/sys/class/invensense/mpu/gyro_fifo_enable", - .key = "/sys/class/invensense/mpu/key", - .raw_accel = "/sys/class/invensense/mpu/raw_accl", - .reg_dump = "/sys/class/invensense/mpu/reg_dump", - .tap_on = "/sys/class/invensense/mpu/tap_on", - .dmp_firmware = "/sys/class/invensense/mpu/dmp_firmware" - }, - - { /* BMA250 */ - .buffer = "/dev/input/input/event1", - .enable = "/sys/devices/virtual/input/input1/enable", - .raw_data = "/sys/devices/virtual/input/input1/value", - .temperature = NULL, - .fifo_rate = NULL, - .power_state = NULL, - .fsr = NULL, - .lpf = NULL, - .scale = NULL, - .temp_scale = NULL, - .temp_offset = NULL, - .self_test = NULL, - .accel_en = NULL, - .accel_fifo_en = NULL, - .accel_fs = NULL, - .clock_source = NULL, - .early_suspend_en = NULL, - .firmware_loaded = NULL, - .gyro_en = NULL, - .gyro_fifo_en = NULL, - .key = NULL, - .raw_accel = NULL, - .reg_dump = NULL, - .tap_on = NULL, - .dmp_firmware = NULL - } -}; - -static void (*s_func_cb) (void); - -int inv_read_data(char *names, char *data) -{ - char str[8]; - int count; - short s_data; - - count = inv_sysfs_read((char*)names, sizeof(str), str); - if (count < 0) - return count; - count = sscanf(str, "%hd", &s_data); - *data = s_data; - if (count < 1) - return -EAGAIN; - return count; - -} - -void fifoCB(void) -{ - if (1) { - float gyro[3]; - float accel[3]; - float orient[3]; - float rv[3]; - - int8_t accuracy; - inv_time_t timestamp; - - printf("/*************************************************\n"); - inv_get_sensor_type_gyroscope(gyro, &accuracy, ×tamp); - printf("Gyro %13.6f %13.4f %13.4f %5d %9lld\n", - gyro[0], - gyro[1], - gyro[2], - accuracy, - timestamp); - - inv_get_sensor_type_accelerometer(accel, &accuracy, ×tamp); - printf("Accel %13.6f %13.4f %13.4f %5d %9lld\n", - accel[0], - accel[1], - accel[2], - accuracy, - timestamp); - - inv_get_sensor_type_rotation_vector(rv, &accuracy, ×tamp); - printf("RV %7.3f %7.3f %7.3f %5d %9lld\n", - rv[0],rv[1],rv[2],accuracy,timestamp); - - inv_get_sensor_type_orientation(orient, &accuracy, ×tamp); - printf("Orientation %7.3f %7.3f %7.3f %5d %9lld\n", - orient[0],orient[1],orient[2],accuracy,timestamp); - printf("/*************************************************\n"); - } -} - -unsigned short orient; -signed char g_gyro_orientation[9] = {1, 0, 0, - 0, 1, 0, - 0, 0, 1}; - -signed char g_accel_orientation[9] = {-1, 0, 0, - 0, -1, 0, - 0, 0, 1}; -float scale; -float range; -long sens; - - - -short mTempOffset = 0; -short mTempScale = 0; -bool mFirstRead = 1; - -/******************* FUNCTIONS *******************************/ -#if 0 -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; -} -#endif - -inv_error_t inv_set_fifo_processed_callback(void (*func_cb)(void)) -{ - s_func_cb = func_cb; - return INV_SUCCESS; -} - -/** - * @brief Keyboard hit function. - */ -int kbhit(void) -{ -#if 1 - struct timeval tv; - fd_set read_fd; - - tv.tv_sec=0; - tv.tv_usec=0; - FD_ZERO(&read_fd); - FD_SET(0,&read_fd); - - if (select(1, &read_fd, NULL, NULL, &tv) == -1) - return 0; - - if (FD_ISSET(0,&read_fd)) - return 1; - - return 0; -#else - struct timeval tv; - fd_set rdfs; - - tv.tv_sec = 0; - tv.tv_usec = 0; - - FD_ZERO(&rdfs); - FD_SET (STDIN_FILENO, &rdfs); - - select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv); - return FD_ISSET(STDIN_FILENO, &rdfs); -#endif -} - -inv_error_t inv_constructor_default_enable() -{ - inv_error_t result; - - result = inv_enable_quaternion(); - if (result) { - if (result == INV_ERROR_NOT_AUTHORIZED) { - LOGE("Enable Quaternion failed: not authorized"); - } - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_motion_no_motion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_gyro_tc(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_hal_outputs(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - result = inv_enable_9x_sensor_fusion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - return result; -} - -int read_attribute_sensor(int fd, char *data, unsigned int size) -{ - int count = 0; - if (fd >=0) { - count = read(fd, data, size); - if(count < 0) { - MPL_LOGE("read fails with error code=%d", count); - } - close(fd); - } - return count; -} - -int inv_read_temperature(long long *data) -{ - int count = 0; - int fd; - - if(mFirstRead) { - char buf[4]; - fd = open(filenames[ITG3500].temp_scale, O_RDONLY); - if(fd < 0) { - MPL_LOGE("errors opening tempscale"); - return -1; - } - - memset(buf, 0, sizeof(buf)); - - count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 0) { - MPL_LOGE("errors reading temp_scale"); - return -1; - } - - count = sscanf(buf, "%hd", &mTempScale); - if(count < 1) - return -1; - MPL_LOGI("temp scale = %d", mTempScale); - - fd = open(filenames[ITG3500].temp_offset, O_RDONLY); - if(fd < 0) { - MPL_LOGE("errors opening tempoffset"); - return -1; - } - - memset(buf, 0, sizeof(buf)); - - count = read_attribute_sensor(fd, buf, sizeof(buf)); - if(count < 0) { - MPL_LOGE("errors reading temp_offset"); - return -1; - } - - count = sscanf(buf, "%hd", &mTempOffset); - if(count < 1) - return -1; - MPL_LOGI("temp offset = %d", mTempOffset); - - mFirstRead = false; - } - - char raw_buf[25]; - short raw; - long long timestamp; - fd = open(filenames[ITG3500].temperature, O_RDONLY); - if(fd < 0) { - MPL_LOGE("errors opening temperature"); - return -1; - } - - memset(raw_buf, 0, sizeof(raw_buf)); - - count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); - if(count < 0) { - MPL_LOGE("errors reading temperature"); - return -1; - } - count = sscanf(raw_buf, "%hd%lld", &raw, ×tamp); - if(count < -1) - return -1; - MPL_LOGI("temperature raw = %d, timestamp = %lld", raw, timestamp); - MPL_LOGI("temperature offset = %d", mTempOffset); - MPL_LOGI("temperature scale = %d", mTempScale); - int adjuster = 35 + ((raw-mTempOffset)/mTempScale); - MPL_LOGI("pre-scaled temperature = %d", adjuster); - MPL_LOGI("adjusted temperature = %d", adjuster*65536); - //data[0] = adjuster * 65536; - data[0] = (35 + ((raw - mTempOffset) / mTempScale)) * 65536.f; - data[1] = timestamp; - return 0; -} - -int self_test(void) -{ - int err = 0; - char str[50]; - char x[9], y[9], z[9]; - char pass[2]; - int fd; - - fd = open((char*)filenames[DEVICE].self_test, O_RDONLY); - if(fd < 0) { - return fd; - } - memset(str, 0, sizeof(str)); - err = read_attribute_sensor(fd, str, sizeof(str)); - if(err < 0) { - return err; - } - MPL_LOGI("self_test result: %s", str); - printf("Self test result: %s ", str); - err = sscanf(str, "%[^','],%[^','],%[^','],%[^',']", x, y, z, pass); - if(err < 1) { - return err; - } - MPL_LOGI("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); - //printf("Bias : X:Y:Z (%ld, %ld, %ld)", atol(x), atol(y), atol(z)); - if (atoi(pass)) { - MPL_LOGI("Result : PASS (1)"); - printf("----> PASS (1)\n"); - } else { - MPL_LOGI("Result : FAIL (0)"); - printf("----> FAIL (0)\n"); - } - return err; -} - -/******************************************************************************* - ******************************* MAIN ****************************************** - ******************************************************************************/ - -/** - * @brief Main function - */ -int main(int argc, char *argv[]) -{ - int key = 0; - int ready; - long accel[3]; - short gyro[3]; - long long timestamp = 0; - inv_error_t result; - - char data; - unsigned char i; - int fd, bytes_read; - struct pollfd pfd; - unsigned long long time_stamp; - unsigned int time_H; - struct input_event ev[100]; -#ifdef INV_PLAYBACK_DBG - int logging = false; - FILE *logfile = NULL; -#endif - - result = inv_init_mpl(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - // Master Enabling. This also turns on power_state - if (inv_sysfs_write((char *)filenames[DEVICE].enable, 1) < 0) - printf("ERR- Failed to enable event generation\n"); - else { - inv_read_data((char *)filenames[DEVICE].enable, &data); - printf("Event enable= %d\n", data); - } - - // Power ON - No need after master enable above but do it anyway - if (inv_sysfs_write((char *)filenames[DEVICE].power_state, 1) < 0) - printf("ERR- Failed to set power state=1\n"); - else { - inv_read_data((char *)filenames[DEVICE].power_state, &data); - printf("Power state: %d\n", data); - } - - // Turn on tap - if (inv_sysfs_write((char *)filenames[DEVICE].tap_on, 1) < 0) { - printf("ERR- Failed to enable Tap On\n"); - } - else { - inv_read_data((char *)filenames[DEVICE].tap_on, &data); - printf("Tap-on: %d\n", data); - } - - // Program DMP code. No longer required to enable tap-on first - if ((result = - inv_sysfs_write((char *)filenames[DEVICE].firmware_loaded, 0)) < 0) { - printf("ERR- Failed to initiate DMP re-programming %d\n",result); - } else { - if ((fd = open(filenames[DEVICE].dmp_firmware, O_WRONLY)) < 0 ) { - printf("ERR- Failed file open to write DMP\n"); - close(fd); - exit(0); - } else { - // Program 200Hz version - //result = write(fd, DMP_IMAGE, sizeof(DMP_IMAGE)); - //printf("Downloaded %d byte(s) to DMP\n", result); - result = inv_load_dmp(fd); - //LOG_RESULT_LOCATION(result); - close(fd); - } - } - - // Query DMP running. For now check by 'firmware_loaded' status - if (inv_read_data((char *)filenames[DEVICE].firmware_loaded, &data) < 0) { - printf("ERR- Failed to read 'firmware_loaded'\n"); - } else { - printf("Firmware Loaded/ DMP running: %d\n", data); - } - - inv_set_fifo_processed_callback(fifoCB); - result = inv_constructor_default_enable(); - result = inv_start_mpl(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else { - printf ("MPL started\n"); - } - - /* Gyro Setup */ - orient = inv_orientation_matrix_to_scalar(g_gyro_orientation); - inv_set_gyro_orientation_and_scale(orient,2000L<<15); - - /* Accel Setup */ - orient = inv_orientation_matrix_to_scalar(g_accel_orientation); - /* NOTE: sens expected to be 2 (FSR) * 1L<<15 for 16 bit hardware data. - * The BMA250 only uses a 10 bit ADC, so we shift the data by 6 bits. - * 2 * 1L<<15 * 1<<6 == 1LL<<22 - */ - inv_set_accel_orientation_and_scale(orient, 1LL<<22); - - // Enable Gyro - if (inv_sysfs_write((char *)filenames[DEVICE].gyro_en, 1) <0) - printf("ERR- Failed to enable Gyro\n"); - else { - inv_read_data((char *)filenames[DEVICE].gyro_en, &data); - printf("Gyro enable: %d\n", data); - } - - // Enable Accel - if (inv_sysfs_write((char *)filenames[DEVICE].accel_en, 1) <0) - printf("ERR- Failed to enable Accel\n"); - else { - inv_read_data((char *)filenames[DEVICE].accel_en, &data); - printf("Accel enable: %d\n", data); - } - - // polling for data - fd = open(filenames[DEVICE].buffer, O_RDONLY); - if(fd < 0) { - MPL_LOGE("Cannot open device event buffer"); - } - - pfd.fd = fd; - pfd.events = POLLIN; - - while (1) { - - result = kbhit(); - if (result) { - key = getchar(); - } else { - key = 0; - } - if (key == 'l') { - MPL_LOGI(" 'l' - load calibration file"); - inv_load_calibration(); - } - if (key == 't') { - MPL_LOGI(" 't' - self test"); - self_test(); - } - if (key == 'q') { - MPL_LOGI(" 'q' - store calibration file"); - inv_store_calibration(); - break; - } -#ifdef INV_PLAYBACK_DBG - if (key == 's') { - if (!logging) { - MPL_LOGI(" 's' - toggle logging on"); - logfile = fopen("/data/playback.bin", "wb"); - if (logfile) { - inv_turn_on_data_logging(logfile); - logging = true; - } else { - MPL_LOGI("Error : " - "cannot open log file '/data/playback.bin'"); - } - } else { - MPL_LOGI(" 's' - toggle logging off"); - inv_turn_off_data_logging(); - fclose(logfile); - logging = false; - } - break; - } -#endif - - ready = poll(&pfd, 1, 100); - if (ready) { - bytes_read = read_attribute_sensor(fd, (char *)ev, - sizeof(struct input_event) * SIX_AXES); - //bytes_read= read(fd, &ev, sizeof(struct input_event) * SIX_AXES); - if (bytes_read > 0) { - int executed; - - for (i = 0; i < bytes_read / sizeof(struct input_event); i++) { - if (ev[i].type == EV_REL) { - switch (ev[i].code) { - case REL_X: - printf("REL_X\n"); - gyro[0]= ev[i].value; //Gyro X - printf("Gyro X:%5d ", gyro[0]); - break; - case REL_Y: - printf("REL_Y\n"); - gyro[1]= ev[i].value; //Gyro Y - printf("Gyro Y:%5d ", gyro[1]); - break; - case REL_Z: - printf("REL_Z\n"); - gyro[2]= ev[i].value; //Gyro Z - printf("Gyro Z:%5d ", gyro[2]); - break; - case REL_RX: - printf("REL_RX\n"); - accel[0]= ev[i].value; //Accel X - printf("Accl X:%5ld ", accel[0]); - break; - case REL_RY: - printf("REL_RY\n"); - accel[1]= ev[i].value; //Accel Y - printf("Accl Y:%5ld ", accel[1]); - break; - case REL_RZ: - printf("REL_RZ\n"); - accel[2]= ev[i].value; //Accel Z - printf("Accl Z:%5ld ", accel[2]); - break; - case REL_MISC: - time_H= ev[i].value; - break; - case REL_WHEEL: - time_stamp = ((unsigned long long)(time_H) << 32) + - (unsigned int)ev[i].value; - break; - default: - printf("ERR- Un-recognized event code: %5d ", ev[i].code); - break; - } - } else { -#if 0 - clock_gettime(CLOCK_MONOTONIC, &timer); - curr_time= timer.tv_nsec + timer.tv_sec * 1000000000LL; - printf("Curr time= %lld, Dev time stamp= %lld, Time diff= %d ms\n", curr_time, time_stamp, (curr_time-time_stamp)/1000000LL); -#endif - } - } - - // build & process gyro + accel data - result = inv_build_gyro(gyro, (inv_time_t)timestamp, &executed); - if (result) { - LOG_RESULT_LOCATION(result); - } else if ((result = inv_build_accel(accel, 0, - (inv_time_t)timestamp, - &executed))) { - LOG_RESULT_LOCATION(result); - } - if (executed) { - printf("Exec on data Ok\n"); - s_func_cb(); - } - - } else { - //printf ("ERR- No data!\n"); - } - - } else { MPL_LOGV("Device not ready"); } - } - close(fd); - -#ifdef INV_PLAYBACK_DBG - if (logging) { - inv_turn_off_data_logging(); - fclose(logfile); - } -#endif - - return 0; -} diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared b/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared deleted file mode 100644 index 5d52b21..0000000 Binary files a/libsensors_iio/software/simple_apps/input_sub/build/android/input_gyro-shared and /dev/null differ diff --git a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk b/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk deleted file mode 100644 index 7f6cc43..0000000 --- a/libsensors_iio/software/simple_apps/input_sub/build/android/shared.mk +++ /dev/null @@ -1,109 +0,0 @@ -EXEC = input_gyro$(SHARED_APP_SUFFIX) - -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 = ../../../../.. -APP_DIR = $(CURDIR)/../.. -MLLITE_DIR = $(INV_ROOT)/software/core/mllite -COMMON_DIR = $(INV_ROOT)/software/simple_apps/common -MPL_DIR = $(INV_ROOT)/software/core/mpl -HAL_DIR = $(INV_ROOT)/software/core/HAL - -include $(INV_ROOT)/software/build/android/common.mk - -CFLAGS += $(CMDLINE_CFLAGS) -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$(MPL_DIR) -CFLAGS += -I$(COMMON_DIR) -CFLAGS += -I$(HAL_DIR)/include -CFLAGS += $(INV_INCLUDES) -CFLAGS += $(INV_DEFINES) - -LLINK = -lc -LLINK += -lm -LLINK += -lutils -LLINK += -lcutils -LLINK += -lgcc -LLINK += -ldl -LLINK += -lstdc++ -LLINK += -llog -LLINK += -lz - -PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x -PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o -PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o - -LFLAGS += $(CMDLINE_LFLAGS) -LFLAGS += -nostdlib -LFLAGS += -fpic -LFLAGS += -Wl,--gc-sections -LFLAGS += -Wl,--no-whole-archive -LFLAGS += -Wl,-dynamic-linker,/system/bin/linker -LFLAGS += $(ANDROID_LINK) -ifneq ($(PRODUCT),panda) -LFLAGS += -rdynamic -endif - -LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib - -#################################################################################################### -## sources - -INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) -INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) - -#INV_SOURCES and 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 clean cleanall install - -all: $(EXEC) $(MK_NAME) - -$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) - @$(call echo_in_colors, "\n\n") - mkdir obj - -$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) - @$(call echo_in_colors, "\n\n") - $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< - -clean : - rm -fR $(OBJFOLDER) - -cleanall : - rm -fR $(EXEC) $(OBJFOLDER) - -install : $(EXEC) - cp -f $(EXEC) $(INSTALL_DIR) - - diff --git a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk b/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk deleted file mode 100644 index 0936212..0000000 --- a/libsensors_iio/software/simple_apps/input_sub/build/filelist.mk +++ /dev/null @@ -1,13 +0,0 @@ -#### filelist.mk for input_gyro #### - -# helper headers -HEADERS := $(MPL_DIR)/authenticate.h -#HEADERS += - -# sources -SOURCES := $(APP_DIR)/test_input_gyro.c - -INV_SOURCES += $(SOURCES) - -#VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux -VPATH += $(APP_DIR) diff --git a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c b/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c deleted file mode 100644 index 6fa9aab..0000000 --- a/libsensors_iio/software/simple_apps/input_sub/test_input_gyro.c +++ /dev/null @@ -1,485 +0,0 @@ -/* - * input interface testing - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "linux/ml_sysfs_helper.h" -#include "authenticate.h" -#include "ml_load_dmp.h" - -#if 0 -struct input_event { - struct timeval time; - __u16 type; - __u16 code; - __s32 value; -}; -#endif - -void HandleOrient(int orient) -{ - if (orient & 0x01) - printf("INV_X_UP\n"); - if (orient & 0x02) - printf("INV_X_DOWN\n"); - if (orient & 0x04) - printf("INV_Y_UP\n"); - if (orient & 0x08) - printf("INV_Y_DOWN\n"); - if (orient & 0x10) - printf("INV_Z_UP\n"); - if (orient & 0x20) - printf("INV_Z_DOWN\n"); - if (orient & 0x40) - printf("INV_ORIENTATION_FLIP\n"); -} - -void HandleTap(int tap) -{ - int tap_dir = tap/8; - int tap_num = tap%8 + 1; - - switch (tap_dir) { - case 1: - printf("INV_TAP_AXIS_X_POS\n"); - break; - case 2: - printf("INV_TAP_AXIS_X_NEG\n"); - break; - case 3: - printf("INV_TAP_AXIS_Y_POS\n"); - break; - case 4: - printf("INV_TAP_AXIS_Y_NEG\n"); - break; - case 5: - printf("INV_TAP_AXIS_Z_POS\n"); - break; - case 6: - printf("INV_TAP_AXIS_Z_NEG\n"); - break; - default: - break; - } - printf("Tap number: %d\n", tap_num); -} - -static void read_compass(int event_number) -{ - int ev_size, ret_byte, ii; - int fd = -1; - struct input_event ev[10]; - char name[64]; - char file_name[64]; - unsigned int RX; - unsigned long long time0, time1, time2; - struct timespec tsp; - ev_size = sizeof(struct input_event); - sprintf(file_name, "/dev/input/event%d", event_number); - if ((fd = open(file_name, O_RDONLY)) < 0 ) { - printf("fail to open compass\n"); - return; - } - - /* NOTE: Use this to pass device name to HAL. */ - ioctl (fd, EVIOCGNAME (sizeof (name)), name); - printf ("Reading From : (%s)\n", name); - while (1) { - clock_gettime(CLOCK_MONOTONIC, &tsp); - /*read compass data here */ - if(fd > 0){ - ret_byte = read(fd, ev, ev_size); - } else { - ret_byte = -1; - } - time0 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL; - if (ret_byte < 0) - continue; - for (ii = 0; ii < ret_byte/ev_size; ii++) { - if(EV_REL != ev[ii].type) { - time2 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL; - printf("mono=%lldms, diff=%d\n", time2, (int)(time1-time0)); - continue; - } - switch (ev[ii].code) { - case REL_X: - printf("CX:%5d ", ev[ii].value); - break; - case REL_Y: - printf("CY:%5d ", ev[ii].value); - break; - case REL_Z: - printf("CZ:%5d ", ev[ii].value); - break; - case REL_MISC: - RX = ev[ii].value; - break; - case REL_WHEEL: - time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value; - time1 = time1/1000000; - printf("time1: %lld ", time1); - break; - default: - printf("GES?: %5d ", ev[ii].code); - break; - } - } - } - close(fd); -} - -static void read_gesture(int num) -{ - int ev_size, ret_byte, ii; - int fd = -1; - struct input_event ev[10]; - char name[64]; - char file_name[64]; - unsigned long long time; - struct timespec tsp; - ev_size = sizeof(struct input_event); - sprintf(file_name, "/dev/input/event%d", num); - MPL_LOGI("%s\n", file_name); - if ((fd = open(file_name, O_RDONLY)) < 0 ) { - printf("fail to open gusture.\n"); - return; - } - - /* NOTE: Use this to pass device name to HAL. */ - ioctl (fd, EVIOCGNAME (sizeof (name)), name); - printf ("Reading From : (%s)\n", name); - while(1){ - clock_gettime(CLOCK_MONOTONIC, &tsp); - if(fd > 0){ - ret_byte = read(fd, ev, ev_size); - } else { - ret_byte = -1; - } - time = tsp.tv_nsec + tsp.tv_sec * 1000000000LL; - //printf("retbyte=%d, ev3=%d\n", ret_byte, ev_size*3); - if (ret_byte < 0) - continue; - for (ii = 0; ii < ret_byte/ev_size; ii++) { - if(EV_REL != ev[ii].type) { - time = ev[ii].time.tv_usec + ev[ii].time.tv_sec * 1000000LL; - printf("mono=%lld\n", time); - continue; - } - switch (ev[ii].code) { - case REL_RX: - printf("GESX:%5x\n", ev[ii].value); - HandleTap(ev[ii].value); - break; - case REL_RY: - printf("GESY:%5x\n", ev[ii].value); - HandleOrient(ev[ii].value); - break; - case REL_RZ: - printf("FLICK:%5x\n", ev[ii].value); - break; - default: - printf("?: %5d ", ev[ii].code); - break; - } - } - } -} - -static void read_gyro_accel(int num) -{ - int ev_size, ret_byte, ii; - int fd = -1; - unsigned int RX; - struct input_event ev[10]; - char name[64]; - char file_name[64]; - unsigned long long time0, time1, time2; - struct timespec tsp; - ev_size = sizeof(struct input_event); - sprintf(file_name, "/dev/input/event%d", num); - if ((fd = open(file_name, O_RDONLY)) < 0 ) { - printf("fail to open gyro/accel\n"); - return; - } - - /* NOTE: Use this to pass device name to HAL. */ - ioctl (fd, EVIOCGNAME (sizeof (name)), name); - printf ("Reading From : (%s)\n", name); - while (1){ - //usleep(20000); - ret_byte = read(fd, ev, ev_size); - if (ret_byte < 0) - continue; - //ret_byte = 0; - - for (ii = 0; ii < ret_byte/ev_size; ii++) { - if(EV_REL != ev[ii].type) { - time0 = ev[ii].time.tv_usec/1000 + ev[ii].time.tv_sec * 1000LL; - printf("T: %lld diff=%d ", time0, (int)(time1 - time0)); - clock_gettime(CLOCK_MONOTONIC, &tsp); - time2 = tsp.tv_nsec/1000000 + tsp.tv_sec * 1000LL; - printf("mono=%lld, diff2=%d\n", time2, (int)(time1 - time2)); - continue; - } - switch (ev[ii].code) { - case REL_X: - printf("GX:%5d ", ev[ii].value); - break; - case REL_Y: - printf("GY:%5d ", ev[ii].value); - break; - case REL_Z: - printf("GZ:%5d ", ev[ii].value); - break; - case REL_RX: - printf("AX:%5d ", ev[ii].value); - break; - case REL_RY: - printf("AY:%5d ", ev[ii].value); - break; - case REL_RZ: - printf("AZ:%5d ", ev[ii].value); - break; - case REL_MISC: - RX = ev[ii].value; - break; - case REL_WHEEL: - time1 = ((unsigned long long)(RX)<<32) + (unsigned int)ev[ii].value; - time1 = time1/1000000; - printf("time1: %lld ", time1); - break; - default: - printf("?: %5d ", ev[ii].code); - break; - } - } - } - close(fd); -} -int inv_sysfs_write(char *filename, long data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; -} -int inv_sysfs_read(char *filename, long num_bytes, char *data) -{ - FILE *fp; - int count; - - if (!filename) - return -1; - fp = fopen(filename, "r"); - if (!fp) - return -errno; - count = fread(data, 1, num_bytes, fp); - fclose(fp); - return count; -} - -void enable_flick(char *p) -{ - char sysfs_file[200]; - printf("flick:%s\n", p); - sprintf(sysfs_file, "%s/flick_int_on", p); - inv_sysfs_write(sysfs_file, 1); - sprintf(sysfs_file, "%s/flick_upper", p); - inv_sysfs_write(sysfs_file, 3147790); - sprintf(sysfs_file, "%s/flick_lower", p); - inv_sysfs_write(sysfs_file, -3147790); - sprintf(sysfs_file, "%s/flick_counter", p); - inv_sysfs_write(sysfs_file, 50); - sprintf(sysfs_file, "%s/flick_message_on", p); - inv_sysfs_write(sysfs_file, 0); - sprintf(sysfs_file, "%s/flick_axis", p); - inv_sysfs_write(sysfs_file, 2); -} - -void setup_dmp(char *sysfs_path) -{ - char sysfs_file[200]; - char firmware_loaded[200], dmp_path[200]; - char dd[10]; - - inv_get_dmpfile(dmp_path); - sprintf(sysfs_file, "%s/fifo_rate", sysfs_path); - inv_sysfs_write(sysfs_file, 200); - sprintf(sysfs_file, "%s/FSR", sysfs_path); - inv_sysfs_write(sysfs_file, 2000); - sprintf(sysfs_file, "%s/accl_fs", sysfs_path); - inv_sysfs_write(sysfs_file, 4); - /* - sprintf(firmware_loaded, "%s/%s", sysfs_path, "firmware_loaded"); - printf("%s\n", firmware_loaded); - inv_sysfs_write(firmware_loaded, 0); - inv_sysfs_read(firmware_loaded, 1, dd); - printf("beforefirmware_loaded=%c\n", dd[0]); - - if ((fd = open(dmp_path, O_WRONLY)) < 0 ) { - perror("dmp fail"); - } - inv_load_dmp(fd); - close(fd); - */ - inv_sysfs_read(firmware_loaded, 1, dd); - printf("firmware_loaded=%c\n", dd[0]); -} -void read_pedometer(char *sysfs_path){ - int steps; - char sysfs_file[200]; - char dd[4]; - sprintf(sysfs_file, "%s/pedometer_steps", sysfs_path); - inv_sysfs_read(sysfs_file, 4, dd); - steps = dd[0] << 8 | dd[1]; - printf("fff=%d\n", steps); -} -/* The running sequence: - "input_gyro 2 &". - This will setup the dmp firmware and let it run on background. - tap and flick will work at this time. - To see accelerometer data and gyro data. - type : - "input_gyro ". - This will print out gyro data and accelerometer data - To see Compass data - type: - "input_gyro 1" */ - -int main(int argc, char *argv[]) -{ - unsigned int RX, i, sel; - unsigned char key[16]; - struct timeval tv; - struct timespec tsp0, tsp1, tsp2, tsp3; - int event_num; - char sysfs_path[200]; - char chip_name[20]; - char sysfs_file[200]; - if (INV_SUCCESS != inv_check_key()) { - printf("key check fail\n"); - exit(0); - }else - printf("key authenticated\n"); - - for(i=0;i<16;i++){ - key[i] = 0xff; - } - RX = inv_get_sysfs_key(key); - if(RX == INV_SUCCESS){ - for(i=0;i<16;i++){ - printf("%d, ", key[i]); - } - printf("\n"); - }else{ - printf("get key failed\n"); - } - memset(sysfs_path, 0, 200); - memset(sysfs_file, 0, 200); - memset(chip_name, 0, 20); - inv_get_sysfs_path(sysfs_path); - inv_get_chip_name(chip_name); - printf("sysfs path: %s\n", sysfs_path); - printf("chip name: %s\n", chip_name); - /*set up driver*/ - sprintf(sysfs_file, "%s/enable", sysfs_path); - inv_sysfs_write(sysfs_file, 0); - sprintf(sysfs_file, "%s/power_state", sysfs_path); - inv_sysfs_write(sysfs_file, 1); - if ((getuid ()) != 0) - printf ("You are not root! This may not work...\n"); - - if(argc ==2 ) - sel = argv[1][0] - 0x30; - else - sel = 0; - switch(sel){ - case 0: - printf("-------------------------------\n"); - printf("--- log gyro and accel data ---\n"); - printf("-------------------------------\n"); - sprintf(sysfs_file, "%s/enable", sysfs_path); - inv_sysfs_write(sysfs_file, 1); - if(inv_get_handler_number(chip_name, &event_num) < 0) - printf("mpu not installed\n"); - else - read_gyro_accel(event_num); - break; - - case 1: - printf("------------------------\n"); - printf("--- log compass data ---\n"); - printf("------------------------\n"); - sprintf(sysfs_file, "%s/compass_enable", sysfs_path); - inv_sysfs_write(sysfs_file, 1); - sprintf(sysfs_file, "%s/enable", sysfs_path); - inv_sysfs_write(sysfs_file, 1); - if(inv_get_handler_number("INV_COMPASS", &event_num) < 0) - printf("compass is not enabled\n"); - else - read_compass(event_num); - break; - - case 2: - printf("--------------------\n"); - printf("--- log gestures ---\n"); - printf("--------------------\n"); - setup_dmp(sysfs_path); - enable_flick(sysfs_path); - sprintf(sysfs_file, "%s/tap_on", sysfs_path); - inv_sysfs_write(sysfs_file, 1); - sprintf(sysfs_file, "%s/enable", sysfs_path); - inv_sysfs_write(sysfs_file, 1); - if(inv_get_handler_number("INV_DMP", &event_num) < 0) - printf("DMP not enabled\n"); - else - read_gesture(event_num); - break; - - case 3: - printf("-----------------\n"); - printf("--- pedometer ---\n"); - printf("-----------------\n"); - read_pedometer(sysfs_path); - break; - - default: - printf("error choice\n"); - break; - } - - gettimeofday(&tv, NULL); - clock_gettime(CLOCK_MONOTONIC, &tsp1); - clock_gettime(CLOCK_REALTIME, &tsp0); - - clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tsp2); - clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tsp3); - //printf("id=%d, %d, %d, %d\n", CLOCK_MONOTONIC, CLOCK_REALTIME, - // CLOCK_PROCESS_CPUTIME_ID, CLOCK_THREAD_CPUTIME_ID); - //printf("sec0=%lu , nsec=%ld\n", tsp0.tv_sec, tsp0.tv_nsec); - //printf("sec1=%lu , nsec=%ld\n", tsp1.tv_sec, tsp1.tv_nsec); - //printf("sec=%lu , nsec=%ld\n", tsp2.tv_sec, tsp2.tv_nsec); - //printf("sec=%lu , nsec=%ld\n", tsp3.tv_sec, tsp3.tv_nsec); - - //ioctl (fd, EVIOCGNAME (sizeof (name)), name); - //printf ("Reading From : %s (%s)\n", argv[1], name); - - - return 0; -} - diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared new file mode 100644 index 0000000..aab27bb Binary files /dev/null and b/libsensors_iio/software/simple_apps/mpu_iio/build/android/inv_mpu_iio-shared differ diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk new file mode 100644 index 0000000..234c1d9 --- /dev/null +++ b/libsensors_iio/software/simple_apps/mpu_iio/build/android/shared.mk @@ -0,0 +1,109 @@ +EXEC = inv_mpu_iio$(SHARED_APP_SUFFIX) + +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 = ../../../../.. +APP_DIR = $(CURDIR)/../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +COMMON_DIR = $(INV_ROOT)/software/simple_apps/common +MPL_DIR = $(INV_ROOT)/software/core/mpl +HAL_DIR = $(INV_ROOT)/software/core/HAL + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +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$(MPL_DIR) +CFLAGS += -I$(COMMON_DIR) +CFLAGS += -I$(HAL_DIR)/include +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl +LLINK += -lstdc++ +LLINK += -llog +LLINK += -lz + +PRE_LFLAGS := -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o +PRE_LFLAGS += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -nostdlib +LFLAGS += -fpic +LFLAGS += -Wl,--gc-sections +LFLAGS += -Wl,--no-whole-archive +LFLAGS += -Wl,-dynamic-linker,/system/bin/linker +LFLAGS += $(ANDROID_LINK) +ifneq ($(PRODUCT),panda) +LFLAGS += -rdynamic +endif + +LRPATH = -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MPL_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) +INV_LIBS += $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES and 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 clean cleanall install + +all: $(EXEC) $(MK_NAME) + +$(EXEC) : $(OBJFOLDER) $(INV_OBJS_DST) $(INV_LIBS) $(MK_NAME) + @$(call echo_in_colors, "\n\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(INV_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(EXEC) $(OBJFOLDER) + +install : $(EXEC) + cp -f $(EXEC) $(INSTALL_DIR) + + diff --git a/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk new file mode 100644 index 0000000..8a3977a --- /dev/null +++ b/libsensors_iio/software/simple_apps/mpu_iio/build/filelist.mk @@ -0,0 +1,12 @@ +#### filelist.mk for mpu_iio #### + +# headers +#HEADERS += $(HAL_DIR)/include/inv_sysfs_utils.h +HEADERS += $(APP_DIR)/iio_utils.h + +# sources +SOURCES := $(APP_DIR)/mpu_iio.c + +INV_SOURCES += $(SOURCES) + +VPATH += $(APP_DIR) $(COMMON_DIR) $(HAL_DIR)/linux diff --git a/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h new file mode 100644 index 0000000..773ff2c --- /dev/null +++ b/libsensors_iio/software/simple_apps/mpu_iio/iio_utils.h @@ -0,0 +1,643 @@ +/* IIO - useful set of util functionality + * + * Copyright (c) 2008 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +/* Made up value to limit allocation sizes */ +#include +#include +#include +#include +#include +#include + +#define IIO_MAX_NAME_LENGTH 30 + +#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" +#define FORMAT_TYPE_FILE "%s_type" + +const char *iio_dir = "/sys/bus/iio/devices/"; + +/** + * iioutils_break_up_name() - extract generic name from full channel name + * @full_name: the full channel name + * @generic_name: the output generic channel name + **/ +static int iioutils_break_up_name(const char *full_name, + char **generic_name) +{ + char *current; + char *w, *r; + char *working; + current = strdup(full_name); + working = strtok(current, "_\0"); + w = working; + r = working; + + while (*r != '\0') { + if (!isdigit(*r)) { + *w = *r; + w++; + } + r++; + } + *w = '\0'; + *generic_name = strdup(working); + free(current); + + return 0; +} + +/** + * struct iio_channel_info - information about a given channel + * @name: channel name + * @generic_name: general name for channel type + * @scale: scale factor to be applied for conversion to si units + * @offset: offset to be applied for conversion to si units + * @index: the channel index in the buffer output + * @bytes: number of bytes occupied in buffer output + * @mask: a bit mask for the raw output + * @is_signed: is the raw value stored signed + * @enabled: is this channel enabled + **/ +struct iio_channel_info { + char *name; + char *generic_name; + float scale; + float offset; + unsigned index; + unsigned bytes; + unsigned bits_used; + unsigned shift; + uint64_t mask; + unsigned be; + unsigned is_signed; + unsigned enabled; + unsigned location; +}; + +/** + * iioutils_get_type() - find and process _type attribute data + * @is_signed: output whether channel is signed + * @bytes: output how many bytes the channel storage occupies + * @mask: output a bit mask for the raw data + * @be: big endian + * @device_dir: the iio device directory + * @name: the channel name + * @generic_name: the channel type name + **/ +inline int iioutils_get_type(unsigned *is_signed, + unsigned *bytes, + unsigned *bits_used, + unsigned *shift, + uint64_t *mask, + unsigned *be, + const char *device_dir, + const char *name, + const char *generic_name) +{ + FILE *sysfsfp; + int ret; + DIR *dp; + char *scan_el_dir, *builtname, *builtname_generic, *filename = 0; + char signchar, endianchar; + unsigned padint; + const struct dirent *ent; + + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + ret = asprintf(&builtname, FORMAT_TYPE_FILE, name); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_scan_el_dir; + } + ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_builtname; + } + + dp = opendir(scan_el_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_builtname_generic; + } + while (ent = readdir(dp), ent != NULL) + /* + * Do we allow devices to override a generic name with + * a specific one? + */ + if ((strcmp(builtname, ent->d_name) == 0) || + (strcmp(builtname_generic, ent->d_name) == 0)) { + ret = asprintf(&filename, + "%s/%s", scan_el_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_closedir; + } + sysfsfp = fopen(filename, "r"); + if (sysfsfp == NULL) { + printf("failed to open %s\n", filename); + ret = -errno; + goto error_free_filename; + } + + ret = fscanf(sysfsfp, + "%ce:%c%u/%u>>%u", + &endianchar, + &signchar, + bits_used, + &padint, shift); + if (ret < 0) { + printf("failed to pass scan type description\n"); + return ret; + } + *be = (endianchar == 'b'); + *bytes = padint / 8; + if (*bits_used == 64) + *mask = ~0; + else + *mask = (1 << *bits_used) - 1; + if (signchar == 's') + *is_signed = 1; + else + *is_signed = 0; + fclose(sysfsfp); + free(filename); + + filename = 0; + } +error_free_filename: + if (filename) + free(filename); +error_closedir: + closedir(dp); +error_free_builtname_generic: + free(builtname_generic); +error_free_builtname: + free(builtname); +error_free_scan_el_dir: + free(scan_el_dir); +error_ret: + return ret; +} + +inline int iioutils_get_param_float(float *output, + const char *param_name, + const char *device_dir, + const char *name, + const char *generic_name) +{ + FILE *sysfsfp; + int ret; + DIR *dp; + char *builtname, *builtname_generic; + char *filename = NULL; + const struct dirent *ent; + + ret = asprintf(&builtname, "%s_%s", name, param_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + ret = asprintf(&builtname_generic, + "%s_%s", generic_name, param_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_builtname; + } + dp = opendir(device_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_builtname_generic; + } + while (ent = readdir(dp), ent != NULL) + if ((strcmp(builtname, ent->d_name) == 0) || + (strcmp(builtname_generic, ent->d_name) == 0)) { + ret = asprintf(&filename, + "%s/%s", device_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_closedir; + } + sysfsfp = fopen(filename, "r"); + if (!sysfsfp) { + ret = -errno; + goto error_free_filename; + } + fscanf(sysfsfp, "%f", output); + break; + } +error_free_filename: + if (filename) + free(filename); +error_closedir: + closedir(dp); +error_free_builtname_generic: + free(builtname_generic); +error_free_builtname: + free(builtname); +error_ret: + return ret; +} + +/** + * bsort_channel_array_by_index() - reorder so that the array is in index order + * + **/ + +inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array, + int cnt) +{ + + struct iio_channel_info temp; + int x, y; + + for (x = 0; x < cnt; x++) + for (y = 0; y < (cnt - 1); y++) + if ((*ci_array)[y].index > (*ci_array)[y+1].index) { + temp = (*ci_array)[y + 1]; + (*ci_array)[y + 1] = (*ci_array)[y]; + (*ci_array)[y] = temp; + } +} + +/** + * build_channel_array() - function to figure out what channels are present + * @device_dir: the IIO device directory in sysfs + * @ + **/ +inline int build_channel_array(const char *device_dir, + struct iio_channel_info **ci_array, + int *counter) +{ + DIR *dp; + FILE *sysfsfp; + int count, i; + struct iio_channel_info *current; + int ret; + const struct dirent *ent; + char *scan_el_dir; + char *filename; + + *counter = 0; + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + dp = opendir(scan_el_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_name; + } + while (ent = readdir(dp), ent != NULL) + if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), + "_en") == 0) { + ret = asprintf(&filename, + "%s/%s", scan_el_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_close_dir; + } + sysfsfp = fopen(filename, "r"); + if (sysfsfp == NULL) { + ret = -errno; + free(filename); + goto error_close_dir; + } + fscanf(sysfsfp, "%u", &ret); + printf("%s, %d\n", filename, ret); + if (ret == 1) + (*counter)++; + fclose(sysfsfp); + free(filename); + } + *ci_array = malloc(sizeof(**ci_array) * (*counter)); + if (*ci_array == NULL) { + ret = -ENOMEM; + goto error_close_dir; + } + closedir(dp); + dp = opendir(scan_el_dir); + //seekdir(dp, 0); + count = 0; + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), + "_en") == 0) { + current = &(*ci_array)[count++]; + ret = asprintf(&filename, + "%s/%s", scan_el_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + /* decrement count to avoid freeing name */ + count--; + goto error_cleanup_array; + } + sysfsfp = fopen(filename, "r"); + if (sysfsfp == NULL) { + free(filename); + ret = -errno; + goto error_cleanup_array; + } + fscanf(sysfsfp, "%u", ¤t->enabled); + fclose(sysfsfp); + + if (!current->enabled) { + free(filename); + count--; + continue; + } + + current->scale = 1.0; + current->offset = 0; + current->name = strndup(ent->d_name, + strlen(ent->d_name) - + strlen("_en")); + if (current->name == NULL) { + free(filename); + ret = -ENOMEM; + goto error_cleanup_array; + } + /* Get the generic and specific name elements */ + ret = iioutils_break_up_name(current->name, + ¤t->generic_name); + if (ret) { + free(filename); + goto error_cleanup_array; + } + ret = asprintf(&filename, + "%s/%s_index", + scan_el_dir, + current->name); + if (ret < 0) { + free(filename); + ret = -ENOMEM; + goto error_cleanup_array; + } + sysfsfp = fopen(filename, "r"); + fscanf(sysfsfp, "%u", ¤t->index); + fclose(sysfsfp); + free(filename); + /* Find the scale */ + ret = iioutils_get_param_float(¤t->scale, + "scale", + device_dir, + current->name, + current->generic_name); + if (ret < 0) + goto error_cleanup_array; + ret = iioutils_get_param_float(¤t->offset, + "offset", + device_dir, + current->name, + current->generic_name); + if (ret < 0) + goto error_cleanup_array; + ret = iioutils_get_type(¤t->is_signed, + ¤t->bytes, + ¤t->bits_used, + ¤t->shift, + ¤t->mask, + ¤t->be, + device_dir, + current->name, + current->generic_name); + } + } + + closedir(dp); + /* reorder so that the array is in index order */ + bsort_channel_array_by_index(ci_array, *counter); + + return 0; + +error_cleanup_array: + for (i = count - 1; i >= 0; i--) + free((*ci_array)[i].name); + free(*ci_array); +error_close_dir: + closedir(dp); +error_free_name: + free(scan_el_dir); +error_ret: + return ret; +} + +inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify) +{ + int ret; + FILE *sysfsfp; + int test; + char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + printf("failed to open %s\n", temp); + ret = -errno; + goto error_free; + } + fprintf(sysfsfp, "%d", val); + fclose(sysfsfp); + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + printf("failed to open %s\n", temp); + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%d", &test); + if (test != val) { + printf("Possible failure in int write %d to %s%s\n", + val, + basedir, + filename); + ret = -1; + } + } +error_free: + free(temp); + return ret; +} + +int write_sysfs_int(char *filename, char *basedir, int val) +{ + return _write_sysfs_int(filename, basedir, val, 0); +} + +int write_sysfs_int_and_verify(char *filename, char *basedir, int val) +{ + return _write_sysfs_int(filename, basedir, val, 1); +} + +int _write_sysfs_string(char *filename, char *basedir, char *val, int verify) +{ + int ret = 0; + FILE *sysfsfp; + char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) { + printf("Memory allocation failed\n"); + return -ENOMEM; + } + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + printf("Could not open %s\n", temp); + ret = -errno; + goto error_free; + } + fprintf(sysfsfp, "%s", val); + fclose(sysfsfp); + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + printf("could not open file to verify\n"); + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%s", temp); + if (strcmp(temp, val) != 0) { + printf("Possible failure in string write of %s " + "Should be %s " + "written to %s\%s\n", + temp, + val, + basedir, + filename); + ret = -1; + } + } +error_free: + free(temp); + + return ret; +} + +/** + * write_sysfs_string_and_verify() - string write, readback and verify + * @filename: name of file to write to + * @basedir: the sysfs directory in which the file is to be found + * @val: the string to write + **/ +int write_sysfs_string_and_verify(char *filename, char *basedir, char *val) +{ + return _write_sysfs_string(filename, basedir, val, 1); +} + +int write_sysfs_string(char *filename, char *basedir, char *val) +{ + return _write_sysfs_string(filename, basedir, val, 0); +} + +int read_sysfs_posint(char *filename, char *basedir) +{ + int ret; + FILE *sysfsfp; + char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) { + printf("Memory allocation failed"); + return -ENOMEM; + } + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%d\n", &ret); + fclose(sysfsfp); +error_free: + free(temp); + return ret; +} + +int read_sysfs_float(char *filename, char *basedir, float *val) +{ + float ret = 0; + FILE *sysfsfp; + char *temp = malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) { + printf("Memory allocation failed"); + return -ENOMEM; + } + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%f\n", val); + fclose(sysfsfp); +error_free: + free(temp); + return ret; +} +int enable(const char *device_dir, + struct iio_channel_info **ci_array, + int *counter) +{ + DIR *dp; + int ret; + const struct dirent *ent; + char *scan_el_dir; + + *counter = 0; + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + dp = opendir(scan_el_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_name; + } + while (ent = readdir(dp), ent != NULL) + if (strcmp(ent->d_name + strlen(ent->d_name) - strlen("_en"), + "_en") == 0) { + write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 1); + } + return 0; +error_ret: +error_free_name: + return -1; +} +int disable_q_out(const char *device_dir, + struct iio_channel_info **ci_array, + int *counter) { + DIR *dp; + int ret; + const struct dirent *ent; + char *scan_el_dir; + + *counter = 0; + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + dp = opendir(scan_el_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_name; + } + while (ent = readdir(dp), ent != NULL) + if (strncmp(ent->d_name, "in_quaternion", strlen("in_quaternion")) == 0) { + write_sysfs_int_and_verify((char *)ent->d_name, scan_el_dir, 0); + } + return 0; +error_ret: +error_free_name: + return -1; + +} + diff --git a/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c new file mode 100644 index 0000000..e20d640 --- /dev/null +++ b/libsensors_iio/software/simple_apps/mpu_iio/mpu_iio.c @@ -0,0 +1,582 @@ +/* Industrialio buffer test code. + * + * Copyright (c) 2008 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + * + * This program is primarily intended as an example application. + * Reads the current buffer setup from sysfs and starts a short capture + * from the specified device, pretty printing the result after appropriate + * conversion. + * + * Command line parameters + * generic_buffer -n -t + * If trigger name is not specified the program assumes you want a dataready + * trigger associated with the device and goes looking for it. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "iio_utils.h" +#include "ml_load_dmp.h" +#include "ml_sysfs_helper.h" +#include "authenticate.h" + +/** + * size_from_channelarray() - calculate the storage size of a scan + * @channels: the channel info array + * @num_channels: size of the channel info array + * + * Has the side effect of filling the channels[i].location values used + * in processing the buffer output. + **/ +int size_from_channelarray(struct iio_channel_info *channels, int num_channels) +{ + int bytes = 0; + int i = 0; + while (i < num_channels) { + if (bytes % channels[i].bytes == 0) + channels[i].location = bytes; + else + channels[i].location = bytes - bytes%channels[i].bytes + + channels[i].bytes; + bytes = channels[i].location + channels[i].bytes; + i++; + } + return bytes; +} + +void print2byte(int input, struct iio_channel_info *info) +{ + /* shift before conversion to avoid sign extension + of left aligned data */ + input = input >> info->shift; + if (info->is_signed) { + int16_t val = input; + val &= (1 << info->bits_used) - 1; + val = (int16_t)(val << (16 - info->bits_used)) >> + (16 - info->bits_used); + /*printf("%d, %05f, scale=%05f", val, + (float)(val + info->offset)*info->scale, info->scale);*/ + printf("%d, ", val); + + } else { + uint16_t val = input; + val &= (1 << info->bits_used) - 1; + printf("%05f ", ((float)val + info->offset)*info->scale); + } +} +/** + * process_scan() - print out the values in SI units + * @data: pointer to the start of the scan + * @infoarray: information about the channels. Note + * size_from_channelarray must have been called first to fill the + * location offsets. + * @num_channels: the number of active channels + **/ +void process_scan(char *data, + struct iio_channel_info *infoarray, + int num_channels) +{ + int k; + //char *tmp; + for (k = 0; k < num_channels; k++) { + switch (infoarray[k].bytes) { + /* only a few cases implemented so far */ + case 2: + print2byte(*(uint16_t *)(data + infoarray[k].location), + &infoarray[k]); + //tmp = data + infoarray[k].location; + break; + case 4: + if (infoarray[k].is_signed) { + int32_t val = *(int32_t *) + (data + + infoarray[k].location); + if ((val >> infoarray[k].bits_used) & 1) + val = (val & infoarray[k].mask) | + ~infoarray[k].mask; + /* special case for timestamp */ + printf(" %d ", val); + } + break; + case 8: + if (infoarray[k].is_signed) { + int64_t val = *(int64_t *) + (data + + infoarray[k].location); + if ((val >> infoarray[k].bits_used) & 1) + val = (val & infoarray[k].mask) | + ~infoarray[k].mask; + /* special case for timestamp */ + if (infoarray[k].scale == 1.0f && + infoarray[k].offset == 0.0f) + printf(" %lld", val); + else + printf("%05f ", ((float)val + + infoarray[k].offset)* + infoarray[k].scale); + } + break; + default: + break; + } + } + printf("\n"); +} + +void enable_flick(char *p){ + int ret; + printf("flick:%s\n", p); + ret = write_sysfs_int_and_verify("flick_int_on", p, 1); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("flick_upper", p, 3147790); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("flick_lower", p, -3147790); + if (ret < 0) + return; + + ret = write_sysfs_int_and_verify("flick_counter", p, 50); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("flick_message_on", p, 0); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("flick_axis", p, 0); +} +void HandleOrient(int orient) +{ + if (orient & 0x01) + printf("INV_X_UP\n"); + if (orient & 0x02) + printf("INV_X_DOWN\n"); + if (orient & 0x04) + printf("INV_Y_UP\n"); + if (orient & 0x08) + printf("INV_Y_DOWN\n"); + if (orient & 0x10) + printf("INV_Z_UP\n"); + if (orient & 0x20) + printf("INV_Z_DOWN\n"); + if (orient & 0x40) + printf("INV_ORIENTATION_FLIP\n"); +} + +void HandleTap(int tap) +{ + int tap_dir = tap/8; + int tap_num = tap%8 + 1; + + switch (tap_dir) { + case 1: + printf("INV_TAP_AXIS_X_POS\n"); + break; + case 2: + printf("INV_TAP_AXIS_X_NEG\n"); + break; + case 3: + printf("INV_TAP_AXIS_Y_POS\n"); + break; + case 4: + printf("INV_TAP_AXIS_Y_NEG\n"); + break; + case 5: + printf("INV_TAP_AXIS_Z_POS\n"); + break; + case 6: + printf("INV_TAP_AXIS_Z_NEG\n"); + break; + default: + break; + } + printf("Tap number: %d\n", tap_num); +} +void setup_dmp(char *dev_path){ + char sysfs_path[200]; + char dmp_path[200]; + int ret; + FILE *fd; + sprintf(sysfs_path, "%s", dev_path); + printf("sysfs: %s\n", sysfs_path); + ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1); + if (ret < 0) + return; + + ret = write_sysfs_int("in_accel_scale", dev_path, 0); + if (ret < 0) + return; + ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); + if (ret < 0) + return; + ret = write_sysfs_int("sampling_frequency", sysfs_path, 200); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0); + if (ret < 0) + return; + sprintf(dmp_path, "%s/dmp_firmware", dev_path); + if ((fd = fopen(dmp_path, "wb")) < 0 ) { + perror("dmp fail"); + } + inv_load_dmp(fd); + fclose(fd); + printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path)); + ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1); + if (ret < 0) + return; + enable_flick(sysfs_path); + ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1); + if (ret < 0) + return; + ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1); + if (ret < 0) + return; +} + +void get_dmp_event(char *dev_dir_name) { + char file_name[100]; + int i; + int fp_flick, fp_tap, fp_orient, fp_disp; + int data; + char d[4]; + FILE *fp; + struct pollfd pfd[4]; + printf("%s\n", dev_dir_name); + while(1) { + sprintf(file_name, "%s/event_flick", dev_dir_name); + fp_flick = open(file_name, O_RDONLY | O_NONBLOCK); + sprintf(file_name, "%s/event_tap", dev_dir_name); + fp_tap = open(file_name, O_RDONLY | O_NONBLOCK); + sprintf(file_name, "%s/event_orientation", dev_dir_name); + fp_orient = open(file_name, O_RDONLY | O_NONBLOCK); + sprintf(file_name, "%s/event_display_orientation", dev_dir_name); + fp_disp = open(file_name, O_RDONLY | O_NONBLOCK); + + pfd[0].fd = fp_flick; + pfd[0].events = POLLPRI|POLLERR, + pfd[0].revents = 0; + + pfd[1].fd = fp_tap; + pfd[1].events = POLLPRI|POLLERR, + pfd[1].revents = 0; + + pfd[2].fd = fp_orient; + pfd[2].events = POLLPRI|POLLERR, + pfd[2].revents = 0; + + pfd[3].fd = fp_disp; + pfd[3].events = POLLPRI|POLLERR, + pfd[3].revents = 0; + + read(fp_flick, d, 4); + read(fp_tap, d, 4); + read(fp_orient, d, 4); + read(fp_disp, d, 4); + + poll(pfd, 4, -1); + close(fp_flick); + close(fp_tap); + close(fp_orient); + close(fp_disp); + + for (i=0; i< ARRAY_SIZE(pfd); i++) { + if(pfd[i].revents != 0) { + switch (i){ + case 0: + sprintf(file_name, "%s/event_flick", dev_dir_name); + fp = fopen(file_name, "rt"); + fscanf(fp, "%d\n", &data); + printf("flick=%x\n", data); + fclose(fp); + break; + case 1: + sprintf(file_name, "%s/event_tap", dev_dir_name); + fp = fopen(file_name, "rt"); + fscanf(fp, "%d\n", &data); + printf("tap=%x\n", data); + HandleTap(data); + fclose(fp); + break; + case 2: + sprintf(file_name, "%s/event_orientation", dev_dir_name); + fp = fopen(file_name, "rt"); + fscanf(fp, "%d\n", &data); + printf("orient=%x\n", data); + HandleOrient(data); + fclose(fp); + break; + case 3: + sprintf(file_name, "%s/event_display_orientation", dev_dir_name); + fp = fopen(file_name, "rt"); + fscanf(fp, "%d\n", &data); + printf("display_orient=%x\n", data); + fclose(fp); + break; + } + } + } + } +} + + +int main(int argc, char **argv) +{ + unsigned long num_loops = 2; + unsigned long timedelay = 100000; + unsigned long buf_len = 128; + + int ret, c, i, j, toread; + int fp; + + int num_channels; + char *trigger_name = NULL; + char *dev_dir_name, *buf_dir_name; + + int datardytrigger = 1; + char *data; + int read_size; + int dev_num, trig_num; + char *buffer_access; + int scan_size; + int noevents = 0; + int p_event = 0, nodmp = 0; + char *dummy; + char chip_name[10]; + char device_name[10]; + char sysfs[100]; + + struct iio_channel_info *infoarray; + /*set r means no DMP is enabled. should be used for mpu3050. + set p means no print of data*/ + /* when p is set, 1 means orientation, 2 means tap, 3 means flick*/ + while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) { + switch (c) { + case 't': + trigger_name = optarg; + datardytrigger = 0; + break; + case 'e': + noevents = 1; + break; + case 'p': + p_event = 1; + break; + case 'r': + nodmp = 1; + break; + case 'c': + num_loops = strtoul(optarg, &dummy, 10); + break; + case 'w': + timedelay = strtoul(optarg, &dummy, 10); + break; + case 'l': + buf_len = strtoul(optarg, &dummy, 10); + break; + case '?': + return -1; + } + } + inv_get_sysfs_path(sysfs); + printf("sss:::%s\n", sysfs); + if (inv_get_chip_name(chip_name) != INV_SUCCESS) { + printf("get chip name fail\n"); + exit(0); + } + printf("chip_name=%s\n", chip_name); + if (INV_SUCCESS != inv_check_key()) + printf("key check fail\n"); + else + printf("key authenticated\n"); + + for (i=0; i_dev[n] where n matches the device + * number found above + */ + ret = asprintf(&trigger_name, + "%s-dev%d", device_name, dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + } + ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("firmware_loaded", dev_dir_name, 0); + + /* Verify the trigger exists */ + trig_num = find_type_by_name(trigger_name, "trigger"); + if (trig_num < 0) { + printf("Failed to find the trigger %s\n", trigger_name); + ret = -ENODEV; + goto error_free_triggername; + } + printf("iio trigger number being used is %d\n", trig_num); + /* + * Parse the files in scan_elements to identify what channels are + * present + */ + ret = 0; + ret = enable(dev_dir_name, &infoarray, &num_channels); + if (ret) { + printf("error enable\n"); + goto error_free_triggername; + } + + if (nodmp ==0) + setup_dmp(dev_dir_name); + + /* + * Construct the directory name for the associated buffer. + * As we know that the lis3l02dq has only one buffer this may + * be built rather than found. + */ + ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_triggername; + } + printf("%s %s\n", dev_dir_name, trigger_name); + + /* Set the device trigger to be the data rdy trigger found above */ + ret = write_sysfs_string_and_verify("trigger/current_trigger", + dev_dir_name, + trigger_name); + if (ret < 0) { + printf("Failed to write current_trigger file\n"); + goto error_free_buf_dir_name; + } + /* Setup ring buffer parameters */ + /* length must be even number because iio_store_to_sw_ring is expecting + half pointer to be equal to the read pointer, which is impossible + when buflen is odd number. This is actually a bug in the code */ + ret = write_sysfs_int("length", buf_dir_name, buf_len*2); + if (ret < 0) + goto exit_here; + ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1); + ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1); + if (nodmp == 0) + ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1); + else + ret = disable_q_out(dev_dir_name, &infoarray, &num_channels); + ret = build_channel_array(dev_dir_name, &infoarray, &num_channels); + if (ret) { + printf("Problem reading scan element information\n"); + goto exit_here; + } + + /* Enable the buffer */ + ret = write_sysfs_int("enable", buf_dir_name, 1); + if (ret < 0) + goto exit_here; + scan_size = size_from_channelarray(infoarray, num_channels); + data = malloc(scan_size*buf_len); + if (!data) { + ret = -ENOMEM; + goto exit_here; + } + + ret = asprintf(&buffer_access, + "/dev/iio:device%d", + dev_num); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_data; + } + if (p_event) { + get_dmp_event(dev_dir_name); + goto error_free_buffer_access; + } + /* Attempt to open non blocking the access dev */ + fp = open(buffer_access, O_RDONLY | O_NONBLOCK); + if (fp == -1) { /*If it isn't there make the node */ + printf("Failed to open %s\n", buffer_access); + ret = -errno; + goto error_free_buffer_access; + } + /* Wait for events 10 times */ + for (j = 0; j < num_loops; j++) { + if (!noevents) { + struct pollfd pfd = { + .fd = fp, + .events = POLLIN, + }; + poll(&pfd, 1, -1); + toread = 1; + if ((j%128)==0) + usleep(timedelay); + + } else { + usleep(timedelay); + toread = 1; + } + read_size = read(fp, + data, + toread*scan_size); + if (read_size == -EAGAIN) { + printf("nothing available\n"); + continue; + } + if (0 == p_event) { + for (i = 0; i < read_size/scan_size; i++) + process_scan(data + scan_size*i, + infoarray, + num_channels); + } + } + close(fp); +error_free_buffer_access: + free(buffer_access); +error_free_data: + free(data); +exit_here: + /* Stop the ring buffer */ + ret = write_sysfs_int("enable", buf_dir_name, 0); + +error_free_buf_dir_name: + free(buf_dir_name); +error_free_triggername: + if (datardytrigger) + free(trigger_name); +error_ret: + return ret; +} diff --git a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared index 33c9eef..6dc08bd 100644 Binary files a/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared and b/libsensors_iio/software/simple_apps/self_test/build/android/inv_self_test-shared differ diff --git a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c index 4f9996c..f3eadc4 100644 --- a/libsensors_iio/software/simple_apps/self_test/inv_self_test.c +++ b/libsensors_iio/software/simple_apps/self_test/inv_self_test.c @@ -1,5 +1,5 @@ /** - * Self Test application for Invensense's MPU6050/MPU9150. + * Self Test application for Invensense's MPU6050/MPU6500/MPU9150. */ #include @@ -22,13 +22,19 @@ #include "ml_math_func.h" #include "storage_manager.h" #include "ml_stored_data.h" +#include "ml_sysfs_helper.h" #ifndef ABS #define ABS(x)(((x) >= 0) ? (x) : -(x)) #endif +//#define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */ + +#define MAX_SYSFS_NAME_LEN (100) +#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) + /** Change this key if the data being stored by this file changes */ -#define INV_DB_SAVE_KEY 53394 +#define INV_DB_SAVE_KEY 53395 #define FALSE 0 #define TRUE 1 @@ -38,26 +44,29 @@ #define COMPASS_PASS_STATUS_BIT 0x04 typedef union { - long l; + long l; int i; } bias_dtype; -struct inv_sysfs_names_s { +char *sysfs_names_ptr; + +struct sysfs_attrbs { char *enable; char *power_state; + char *dmp_on; + char *dmp_int_on; char *self_test; char *temperature; - char *accl_bias; -}; - -const struct inv_sysfs_names_s mpu= { - /* MPU6050 & MPU9150 */ - .enable = "/sys/class/invensense/mpu/enable", - .power_state = "/sys/class/invensense/mpu/power_state", - .self_test = "/sys/class/invensense/mpu/self_test", - .temperature = "/sys/class/invensense/mpu/temperature", - .accl_bias = "/sys/class/invensense/mpu/accl_bias" -}; + char *gyro_enable; + char *gyro_x_bias; + char *gyro_y_bias; + char *gyro_z_bias; + char *accel_enable; + char *accel_x_bias; + char *accel_y_bias; + char *accel_z_bias; + char *compass_enable; +} mpu; struct inv_db_save_t { /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ @@ -71,46 +80,123 @@ struct inv_db_save_t { /** 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; }; static struct inv_db_save_t save_data; -/** This function receives the data that was stored in non-volatile memory between power off */ +/** 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(&save_data, data, sizeof(save_data)); return INV_SUCCESS; } -/** This function returns the data to be stored in non-volatile memory between power off */ +/** 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, &save_data, sizeof(save_data)); return INV_SUCCESS; } -int inv_sysfs_write(char *filename, long data) +/** read a sysfs entry that represents an integer */ +int read_sysfs_int(char *filename, int *var) { + int res=0; FILE *fp; - int count; - if (!filename) - return -1; + fp = fopen(filename, "r"); + if (fp != NULL) { + fscanf(fp, "%d\n", var); + fclose(fp); + } else { + LOGE("HAL:ERR open file to read"); + res= -1; + } + return res; +} + +/** write a sysfs entry that represents an integer */ +int write_sysfs_int(char *filename, int data) +{ + int res=0; + FILE *fp; + fp = fopen(filename, "w"); - if (!fp) - return -errno; - count = fprintf(fp, "%ld", data); - fclose(fp); - return count; + if (fp!=NULL) { + fprintf(fp, "%d\n", data); + fclose(fp); + } else { + LOGE("HAL:ERR open file to write"); + res= -1; + } + return res; } -/** - * Main Self test - */ +int inv_init_sysfs_attributes(void) +{ + unsigned char i = 0; + char sysfs_path[MAX_SYSFS_NAME_LEN]; + char *sptr; + char **dptr; + + sysfs_names_ptr = + (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + sptr = sysfs_names_ptr; + if (sptr != NULL) { + dptr = (char**)&mpu; + do { + *dptr++ = sptr; + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < MAX_SYSFS_ATTRB); + } else { + LOGE("HAL:couldn't alloc mem for sysfs paths"); + return -1; + } + + // get proper (in absolute/relative) IIO path & build MPU's sysfs paths + inv_get_sysfs_path(sysfs_path); + + sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable"); + sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); + sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on"); + 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_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias"); + sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias"); + sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias"); + + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); + sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias"); + sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias"); + sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias"); + + sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable"); + +#if 0 + // test print sysfs paths + dptr = (char**)&mpu; + for (i = 0; i < MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} + +/******************************************************************************* + * M a i n S e l f T e s t + ******************************************************************************/ int main(int argc, char **argv) { FILE *fptr; - int self_test_status; + int self_test_status = 0; inv_error_t result; bias_dtype gyro_bias[3]; bias_dtype accel_bias[3]; @@ -119,76 +205,133 @@ int main(int argc, char **argv) int axis_sign = 1; unsigned char *buffer; long timestamp; - int temperature=0; + int temperature = 0; + bool compass_present = TRUE; + + result = inv_init_sysfs_attributes(); + if (result) + return -1; - // Initialize storage manager inv_init_storage_manager(); // Clear out data. memset(&save_data, 0, sizeof(save_data)); - memset(gyro_bias,0, sizeof(gyro_bias)); - memset(accel_bias,0, sizeof(accel_bias)); + memset(gyro_bias, 0, sizeof(gyro_bias)); + memset(accel_bias, 0, sizeof(accel_bias)); // Register packet to be saved. - result = inv_register_load_store(inv_db_load_func, inv_db_save_func, - sizeof(save_data), - INV_DB_SAVE_KEY); + result = inv_register_load_store( + inv_db_load_func, inv_db_save_func, + sizeof(save_data), INV_DB_SAVE_KEY); // Power ON MPUxxxx chip - if (inv_sysfs_write(mpu.power_state, 1) <0) { - printf("ERR- Failed to set power state=1\n"); + if (write_sysfs_int(mpu.power_state, 1) < 0) { + printf("Self-Test:ERR-Failed to set power state=1\n"); } else { // Note: Driver turns on power automatically when self-test invoked } + // Disable Master enable + if (write_sysfs_int(mpu.enable, 0) < 0) { + printf("Self-Test:ERR-Failed to disable master enable\n"); + } + + // Disable DMP + if (write_sysfs_int(mpu.dmp_on, 0) < 0) { + printf("Self-Test:ERR-Failed to disable DMP\n"); + } + + // Enable Accel + if (write_sysfs_int(mpu.accel_enable, 1) < 0) { + printf("Self-Test:ERR-Failed to enable accel\n"); + } + + // Enable Gyro + if (write_sysfs_int(mpu.gyro_enable, 1) < 0) { + printf("Self-Test:ERR-Failed to enable gyro\n"); + } + + // Enable Compass + if (write_sysfs_int(mpu.compass_enable, 1) < 0) { +#ifdef DEBUG_PRINT + printf("Self-Test:ERR-Failed to enable compass\n"); +#endif + compass_present= FALSE; + } + fptr = fopen(mpu.self_test, "r"); - if (fptr != NULL) { - // Invoke self-test and read gyro bias - fscanf(fptr, "%d,%d,%d,%d", - &gyro_bias[0].i, &gyro_bias[1].i, &gyro_bias[2].i, &self_test_status); - - printf("Self-Test:Self test result- Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", - (self_test_status & GYRO_PASS_STATUS_BIT), - (self_test_status & ACCEL_PASS_STATUS_BIT) >>1, - (self_test_status & COMPASS_PASS_STATUS_BIT) >>2); - printf("Self-Test:Gyro bias data[0..2] read from Driver= [%d %d %d]\n",gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); - fclose(fptr); - - if (!(self_test_status & GYRO_PASS_STATUS_BIT)) { - // Reset gyro bias data if gyro self-test failed - memset(gyro_bias,0, sizeof(gyro_bias)); - printf("Self-Test:Failed Gyro self-test\n"); - } + if (!fptr) { + printf("Self-Test:ERR-Couldn't invoke self-test\n"); + result = -1; + goto free_sysfs_storage; + } - if (self_test_status & ACCEL_PASS_STATUS_BIT) { - // Read Accel Bias - fptr= fopen(mpu.accl_bias, "r"); - if (fptr != NULL) { - fscanf(fptr, "%d,%d,%d", &accel_bias[0].i, &accel_bias[1].i, &accel_bias[2].i); - printf("Self-Test:Accel bias data[0..2] read from Driver= [%d %d %d]\n", accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); - fclose(fptr); - } else { - printf("Self-Test:ERR-Couldn't read accel bias\n"); - } + // Invoke self-test + fscanf(fptr, "%d", &self_test_status); + if (compass_present == TRUE) { + printf("Self-Test:Self test result- " + "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", + (self_test_status & GYRO_PASS_STATUS_BIT), + (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1, + (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2); + } else { + printf("Self-Test:Self test result- " + "Gyro passed= %x, Accel passed= %x\n", + (self_test_status & GYRO_PASS_STATUS_BIT), + (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1); + } + fclose(fptr); + + if (self_test_status & GYRO_PASS_STATUS_BIT) { + // Read Gyro Bias + if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 || + read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 || + read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) { + memset(gyro_bias, 0, sizeof(gyro_bias)); + printf("Self-Test:Failed to read Gyro bias\n"); } else { - memset(accel_bias,0, sizeof(accel_bias)); - printf("Self-Test:Failed Accel self-test\n"); + save_data.gyro_accuracy = 3; +#ifdef DEBUG_PRINT + printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n", + gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); +#endif } + } else { + printf("Self-Test:Failed Gyro self-test\n"); + } - // Read temperature - if (self_test_status & (GYRO_PASS_STATUS_BIT|ACCEL_PASS_STATUS_BIT)) - { - fptr= fopen(mpu.temperature, "r"); - if (fptr != NULL) { - fscanf(fptr,"%d %ld", &temperature, ×tamp); - fclose(fptr); - } else { - printf("Self-Test:ERR-Couldn't read temperature\n"); - } - } - + if (self_test_status & ACCEL_PASS_STATUS_BIT) { + // Read Accel Bias + if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 || + read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 || + read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) { + memset(accel_bias,0, sizeof(accel_bias)); + printf("Self-Test:Failed to read Accel bias\n"); + } else { + save_data.accel_accuracy = 3; +#ifdef DEBUG_PRINT + printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n", + accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); +#endif + } } else { - printf("Self-Test:ERR-Couldn't invoke self-test\n"); + printf("Self-Test:Failed Accel self-test\n"); + } + + if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) { + printf("Self-Test:Failed Gyro and Accel self-test, " + "nothing left to do\n"); + result = -1; + goto free_sysfs_storage; + } + + // Read temperature + fptr= fopen(mpu.temperature, "r"); + if (fptr != NULL) { + fscanf(fptr,"%d %ld", &temperature, ×tamp); + fclose(fptr); + } else { + printf("Self-Test:ERR-Couldn't read temperature\n"); } // When we read gyro bias, the bias is in raw units scaled by 1000. @@ -197,7 +340,8 @@ int main(int argc, char **argv) save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f); save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f); - // Save temperature @ time stored. Temperature is in degrees Celsius scaled by 2^16 + // Save temperature @ time stored. + // Temperature is in degrees Celsius scaled by 2^16 save_data.gyro_temp = temperature * (1L << 16); save_data.accel_temp = save_data.gyro_temp; @@ -225,12 +369,16 @@ int main(int argc, char **argv) save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f); #if 1 - printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", save_data.accel_bias[0], - save_data.accel_bias[1], save_data.accel_bias[2]); - printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", save_data.gyro_bias[0], - save_data.gyro_bias[1], save_data.gyro_bias[2]); - printf("Self-Test:Gyro temperature @ time stored %ld\n", save_data.gyro_temp); - printf("Self-Test:Accel temperature @ time stored %ld\n", save_data.accel_temp); + printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", + save_data.accel_bias[0], save_data.accel_bias[1], + save_data.accel_bias[2]); + printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", + save_data.gyro_bias[0], save_data.gyro_bias[1], + save_data.gyro_bias[2]); + printf("Self-Test:Gyro temperature @ time stored %ld\n", + save_data.gyro_temp); + printf("Self-Test:Accel temperature @ time stored %ld\n", + save_data.accel_temp); #endif // Get size of packet to store. @@ -240,25 +388,29 @@ int main(int argc, char **argv) buffer = (unsigned char *)malloc(packet_sz + 10); if (buffer == NULL) { printf("Self-Test:Can't allocate buffer\n"); - return -1; + result = -1; + goto free_sysfs_storage; } + // Store the data result = inv_save_mpl_states(buffer, packet_sz); if (result) { - result= -1; + result = -1; } else { fptr= fopen(MLCAL_FILE, "wb+"); if (fptr != NULL) { fwrite(buffer, 1, packet_sz, fptr); fclose(fptr); } else { - printf("Self-Test:ERR- Can't open calibration file to write - %s\n", + printf("Self-Test:ERR- Can't open calibration file to write - %s\n", MLCAL_FILE); - result= -1; + result = -1; } } - free(buffer); + +free_sysfs_storage: + free(sysfs_names_ptr); return result; } -- cgit v1.2.3