149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Copyright (C) 2012 Invensense, Inc. 349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Licensed under the Apache License, Version 2.0 (the "License"); 549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* you may not use this file except in compliance with the License. 649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* You may obtain a copy of the License at 749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* http://www.apache.org/licenses/LICENSE-2.0 949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* Unless required by applicable law or agreed to in writing, software 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* distributed under the License is distributed on an "AS IS" BASIS, 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* See the License for the specific language governing permissions and 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* limitations under the License. 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 1649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define LOG_NDEBUG 0 1849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow//#define KLP 1 //Key Lime Pie Temporary Test Define 2049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow//see also the EXTRA_VERBOSE define in the MPLSensor.h header file 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <fcntl.h> 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <errno.h> 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <math.h> 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <float.h> 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <poll.h> 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <unistd.h> 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <dirent.h> 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <stdlib.h> 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <sys/select.h> 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <sys/syscall.h> 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <dlfcn.h> 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <pthread.h> 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <cutils/log.h> 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <utils/KeyedVector.h> 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <utils/String8.h> 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <string.h> 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <linux/input.h> 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <utils/Atomic.h> 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "MPLSensor.h" 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "PressureSensor.IIO.secondary.h" 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "MPLSupport.h" 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "sensor_params.h" 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "invensense.h" 4749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "invensense_adv.h" 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_stored_data.h" 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_load_dmp.h" 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_sysfs_helper.h" 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#define ENABLE_MULTI_RATE 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// #define TESTING 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// #define USE_LPQ_AT_FASTEST 554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#define ENABLE_PRESSSURE 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef THIRD_PARTY_ACCEL 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#pragma message("HAL:build third party accel support") 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define USE_THIRD_PARTY_ACCEL (1) 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define USE_THIRD_PARTY_ACCEL (0) 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/******************************************************************************/ 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* MPL interface misc. */ 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/******************************************************************************/ 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic int hertz_request = 200; 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_MPL_GYRO_RATE (20000L) //us 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_MPL_COMPASS_RATE (20000L) //us 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_HW_GYRO_RATE (100) //Hz 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_HW_ACCEL_RATE (20) //ms 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_HW_COMPASS_RATE (20000000L) //ns 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* convert ns to hardware units */ 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define HW_COMPASS_RATE_NS (rate_request) // to ns 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* convert Hz to hardware units */ 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define HW_GYRO_RATE_HZ (hertz_request) 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define HW_ACCEL_RATE_HZ (1000 / hertz_request) 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request) 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define RATE_200HZ 5000000LL 9049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define RATE_15HZ 66667000LL 9149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define RATE_5HZ 200000000LL 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// mask of virtual sensors that require gyro + accel + compass data 94cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#define VIRTUAL_SENSOR_9AXES_MASK ( \ 95cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (1 << Orientation) \ 96cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << RotationVector) \ 97cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << LinearAccel) \ 98cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << Gravity) \ 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow) 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// mask of virtual sensors that require gyro + accel data (but no compass data) 101cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#define VIRTUAL_SENSOR_GYRO_6AXES_MASK ( \ 102cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (1 << GameRotationVector) \ 10349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow) 10449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// mask of virtual sensors that require mag + accel data (but no gyro data) 105cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#define VIRTUAL_SENSOR_MAG_6AXES_MASK ( \ 106cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (1 << GeomagneticRotationVector) \ 10749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow) 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// mask of all virtual sensors 109cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#define VIRTUAL_SENSOR_ALL_MASK ( \ 110cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VIRTUAL_SENSOR_9AXES_MASK \ 111cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | VIRTUAL_SENSOR_GYRO_6AXES_MASK \ 112cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | VIRTUAL_SENSOR_MAG_6AXES_MASK \ 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow) 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct timespec mt_pre; 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct sensor_t sSensorList[] = 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 11849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Gyroscope", "Invensense", 1, 11949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_GYROSCOPE_HANDLE, 120d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 64, 0, 0, 0, 0, {}}, 12149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Raw Gyroscope", "Invensense", 1, 12249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_RAW_GYROSCOPE_HANDLE, 123d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, 2000.0f, 1.0f, 0.5f, 10000, 0, 64, 0, 0, 0, 0, {}}, 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Accelerometer", "Invensense", 1, 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_ACCELERATION_HANDLE, 126d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 64, 0, 0, 0, 0, {}}, 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Magnetic Field", "Invensense", 1, 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_MAGNETIC_FIELD_HANDLE, 129d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 64, 0, 0, 0, 0, {}}, 130cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro {"MPL Raw Magnetic Field", "Invensense", 1, 131cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro SENSORS_RAW_MAGNETIC_FIELD_HANDLE, 132d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, 10240.0f, 1.0f, 0.5f, 10000, 0, 64, 0, 0, 0, 0, {}}, 13349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_PRESSURE 134cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro {"MPL Pressure", "Invensense", 1, 13549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_PRESSURE_HANDLE, 136d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_PRESSURE, 10240.0f, 1.0f, 0.5f, 10000, 0, 64, 0, 0, 0, 0, {}}, 13749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 13849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Orientation", "Invensense", 1, 13949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_ORIENTATION_HANDLE, 140d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}}, 14149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Rotation Vector", "Invensense", 1, 14249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_ROTATION_VECTOR_HANDLE, 143d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, 14449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Game Rotation Vector", "Invensense", 1, 14549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_GAME_ROTATION_VECTOR_HANDLE, 146d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_GAME_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 42, 0, 0, 0, 0, {}}, 14749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Linear Acceleration", "Invensense", 1, 14849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_LINEAR_ACCEL_HANDLE, 149d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, 15049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Gravity", "Invensense", 1, 15149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_GRAVITY_HANDLE, 152d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, 15349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Significant Motion", "Invensense", 1, 15449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_SIGNIFICANT_MOTION_HANDLE, 155d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_SIGNIFICANT_MOTION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}}, 15649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Step Detector", "Invensense", 1, 15749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_PEDOMETER_HANDLE, 158d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_STEP_DETECTOR, 100.0f, 1.0f, 1.1f, 0, 0, 64, 0, 0, 0, 0, {}}, 15949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Step Counter", "Invensense", 1, 16049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_STEP_COUNTER_HANDLE, 161d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_STEP_COUNTER, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}}, 16249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Geomagnetic Rotation Vector", "Invensense", 1, 16349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_GEOMAGNETIC_ROTATION_VECTOR_HANDLE, 164d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, 16549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 16649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow {"MPL Screen Orientation", "Invensense ", 1, 16749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSORS_SCREEN_ORIENTATION_HANDLE, 168d6978c09936e45b596b9347d937c3530bcbfe9b9Aravind Akella SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}}, 16949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 17049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 17149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17249ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowMPLSensor *MPLSensor::gMPLSensor = NULL; 17349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowextern "C" { 17549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid procData_cb_wrapper() 17649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 17749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(MPLSensor::gMPLSensor) { 17849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPLSensor::gMPLSensor->cbProcData(); 17949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 18049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 18149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 18249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid setCallbackObject(MPLSensor* gbpt) 18349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 18449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow MPLSensor::gMPLSensor = gbpt; 18549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 18649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 18749ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowMPLSensor* getCallbackObject() { 18849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return MPLSensor::gMPLSensor; 18949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 19049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} // end of extern C 19249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 193cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro//#define INV_PLAYBACK_DBG 19449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 19549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic FILE *logfile = NULL; 19649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 19749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/******************************************************************************* 19949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * MPLSensor class implementation 20049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ******************************************************************************/ 20149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 20249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// following extended initializer list would only be available with -std=c++11 20349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// or -std=gnu+11 20449ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowMPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *)) 20549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow : SensorBase(NULL, NULL), 20649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mNewData(0), 20749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mMasterSensorMask(INV_ALL_SENSORS), 20849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask(0), 20949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPollTime(-1), 21049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mStepCountPollTime(-1), 21149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHaveGoodMpuCal(0), 21249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroAccuracy(0), 21349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelAccuracy(0), 21449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassAccuracy(0), 21549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mSampleCount(0), 21649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_orient_fd(-1), 21749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpOrientationEnabled(0), 21849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_sign_motion_fd(-1), 21949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpSignificantMotionEnabled(0), 22049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_pedometer_fd(-1), 22149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpPedometerEnabled(0), 22249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpStepCountEnabled(0), 22349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mEnabled(0), 22449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mBatchEnabled(0), 225cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushEnabled(-1), 226cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mOldBatchEnabledMask(0), 22749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelInputReader(4), 22849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroInputReader(32), 22949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mTempScale(0), 23049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mTempOffset(0), 23149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mTempCurrentTime(0), 23249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelScale(2), 233cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelSelfTestScale(2), 23449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroScale(2000), 235cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroSelfTestScale(2000), 23649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassScale(0), 23749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFactoryGyroBiasAvailable(false), 23849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroBiasAvailable(false), 23949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroBiasApplied(false), 24049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFactoryAccelBiasAvailable(false), 24149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelBiasAvailable(false), 24249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelBiasApplied(false), 24349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingMask(0), 24449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mSensorMask(0), 24549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mMplFeatureActiveMask(0), 24649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask(0), 24749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpOn(0), 24849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPedUpdate(0), 249cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPressureUpdate(0), 25049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp(0), 2514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mStepSensorTimestamp(0), 2524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLastStepCount(0), 253cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLeftOverBufferSize(0), 254cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mInitial6QuatValueAvailable(0), 255cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushBatchSet(0), 256cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mSkipReadEvents(0) { 25749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 25849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 25949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t rv; 26049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, fd; 26149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char *port = NULL; 26249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char *ver_str; 26349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned long mSensorMask; 26449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res; 26549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *fptr; 26649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 26749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor = compass; 26849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 26949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, 27049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:MPLSensor constructor : NumSensors = %d", NumSensors); 27149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 27249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pthread_mutex_init(&mMplMutex, NULL); 27349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow pthread_mutex_init(&mHALMutex, NULL); 27449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mGyroOrientation, 0, sizeof(mGyroOrientation)); 27549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mAccelOrientation, 0, sizeof(mAccelOrientation)); 27649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 27749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* setup sysfs paths */ 27849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_init_sysfs_attributes(); 27949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 28049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* get chip name */ 28149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (inv_get_chip_name(chip_ID) != INV_SUCCESS) { 28249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR- Failed to get chip ID\n"); 28349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 28449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID); 28549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 28649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 28749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enable_iio_sysfs(); 28849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 28949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* instantiate pressure sensor on secondary bus */ 29049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (strcmp(mSysfsPath, "") != 0) { 29149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPressureSensor = new PressureSensor((const char*)mSysfsPath); 29249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 29349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR - Failed to instantiate pressure sensor class"); 29449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 29549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 29649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* reset driver master enable */ 29749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(0); 29849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 29949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Always load DMP for KLP 30049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Load DMP image if capable, ie. MPU6xxx/9xxx */ 30149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow loadDMP(); 30249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 30349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* open temperature fd for temp comp */ 30449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature); 30549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_temperature_fd = open(mpu.temperature, O_RDONLY); 30649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_temperature_fd == -1) { 30749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open temperature node"); 30849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 30949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, 31049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:temperature_fd opened: %s", mpu.temperature); 31149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 31249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 31349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* read gyro FSR to calculate accel scale later */ 31449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char gyroBuf[5]; 31549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int count = 0; 31649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, 31749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp()); 31849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 31949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fd = open(mpu.gyro_fsr, O_RDONLY); 32049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fd < 0) { 32149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error opening gyro FSR"); 32249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 32349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(gyroBuf, 0, sizeof(gyroBuf)); 32449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf)); 32549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 1) { 32649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error reading gyro FSR"); 32749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 32849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = sscanf(gyroBuf, "%ld", &mGyroScale); 32949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count) 33049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale); 33149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 33249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(fd); 33349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 3344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 3354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* read gyro self test scale used to calculate factory cal bias later */ 3364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow char gyroScale[5]; 3374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, 3384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp()); 3394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow fd = open(mpu.in_gyro_self_test_scale, O_RDONLY); 3404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(fd < 0) { 3414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:Error opening gyro self test scale"); 3424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 3434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memset(gyroBuf, 0, sizeof(gyroBuf)); 3444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale)); 3454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(count < 1) { 3464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:Error reading gyro self test scale"); 3474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 3484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale); 3494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(count) 3504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale); 3514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 3524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow close(fd); 3534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 35449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 35549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* open Factory Gyro Bias fd */ 3564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* mFactoryGyBias contains bias values that will be used for device offset */ 35749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias)); 35849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset); 35949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset); 3604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset); 36149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR); 36249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR); 36349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR); 36449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_x_offset_fd == -1 || 36549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) { 36649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open factory gyro calibrated bias"); 36749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 36849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, 36949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:gyro_offset opened"); 37049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 37149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 37249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* open Gyro Bias fd */ 3734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* mGyroBias contains bias values that will be used for framework */ 3744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* mGyroChipBias contains bias values that will be used for dmp */ 37549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mGyroBias, 0, sizeof(mGyroBias)); 37649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mGyroChipBias, 0, sizeof(mGyroChipBias)); 37749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias); 37849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias); 37949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias); 38049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR); 38149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR); 38249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR); 38349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_x_dmp_bias_fd == -1 || 38449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) { 38549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open gyro DMP calibrated bias"); 38649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 38749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, 38849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:gyro_dmp_bias opened"); 38949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 39049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 39149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* read accel FSR to calcuate accel scale later */ 39249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (USE_THIRD_PARTY_ACCEL == 0) { 39349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char buf[3]; 39449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int count = 0; 39549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, 39649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp()); 39749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 39849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fd = open(mpu.accel_fsr, O_RDONLY); 39949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fd < 0) { 40049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error opening accel FSR"); 40149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 40249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(buf, 0, sizeof(buf)); 40349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = read_attribute_sensor(fd, buf, sizeof(buf)); 40449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 1) { 40549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error reading accel FSR"); 40649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 40749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = sscanf(buf, "%d", &mAccelScale); 40849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count) 40949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale); 41049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(fd); 41249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 414cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* read accel self test scale used to calculate factory cal bias later */ 415cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro char accelScale[5]; 416cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, 417cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:sysfs:cat %s (%lld)", mpu.in_accel_self_test_scale, getTimestamp()); 418cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro fd = open(mpu.in_accel_self_test_scale, O_RDONLY); 419cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(fd < 0) { 420cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error opening gyro self test scale"); 421cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 422cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memset(buf, 0, sizeof(buf)); 423cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro count = read_attribute_sensor(fd, accelScale, sizeof(accelScale)); 424cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(count < 1) { 425cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error reading accel self test scale"); 426cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 427cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro count = sscanf(accelScale, "%ld", &mAccelSelfTestScale); 428cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(count) 429cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:Accel self test scale used %ld", mAccelSelfTestScale); 430cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 431cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro close(fd); 432cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 433cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 434cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* open Factory Accel Bias fd */ 435cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* mFactoryAccelBias contains bias values that will be used for device offset */ 436cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias)); 437cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel x offset path: %s", mpu.in_accel_x_offset); 438cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel y offset path: %s", mpu.in_accel_y_offset); 439cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel z offset path: %s", mpu.in_accel_z_offset); 440cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_x_offset_fd = open(mpu.in_accel_x_offset, O_RDWR); 441cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_y_offset_fd = open(mpu.in_accel_y_offset, O_RDWR); 442cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_z_offset_fd = open(mpu.in_accel_z_offset, O_RDWR); 443cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (accel_x_offset_fd == -1 || 444cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_y_offset_fd == -1 || accel_z_offset_fd == -1) { 445cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:could not open factory accel calibrated bias"); 446cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 447cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, 448cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:accel_offset opened"); 449cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 450cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 45149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* open Accel Bias fd */ 4524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* mAccelBias contains bias that will be used for dmp */ 45349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mAccelBias, 0, sizeof(mAccelBias)); 45449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias); 45549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias); 45649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias); 45749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR); 45849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR); 45949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR); 46049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accel_x_dmp_bias_fd == -1 || 46149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) { 46249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open accel DMP calibrated bias"); 46349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 46449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, 46549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:accel_dmp_bias opened"); 46649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 46749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 46849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 46949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK); 47049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (dmp_sign_motion_fd < 0) { 47149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR couldn't open dmp_sign_motion node"); 47249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 473cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 474cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd); 47549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 47649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 47749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK); 47849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (dmp_pedometer_fd < 0) { 47949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR couldn't open dmp_pedometer node"); 48049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 481cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 482cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd); 48349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 48449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow initBias(); 4864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 4874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (void)inv_get_version(&ver_str); 488cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGI("%s\n", ver_str); 4894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 4904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* setup MPL */ 4914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_constructor_init(); 4924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 4934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#ifdef INV_PLAYBACK_DBG 4944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); 4954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow logfile = fopen("/data/playback.bin", "w+"); 4964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (logfile) 4974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_turn_on_data_logging(logfile); 4984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#endif 4994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 5004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* setup orientation matrix and scale */ 5014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_device_properties(); 5024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 50349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* initialize sensor data */ 50449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(mPendingEvents, 0, sizeof(mPendingEvents)); 50549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RotationVector].version = sizeof(sensors_event_t); 50749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RotationVector].sensor = ID_RV; 50849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; 50949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RotationVector].acceleration.status 51049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_ACCURACY_HIGH; 51149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t); 51349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GameRotationVector].sensor = ID_GRV; 51449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR; 51549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GameRotationVector].acceleration.status 51649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_ACCURACY_HIGH; 51749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 51849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); 51949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[LinearAccel].sensor = ID_LA; 52049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; 52149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[LinearAccel].acceleration.status 52249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_ACCURACY_HIGH; 52349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 52449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gravity].version = sizeof(sensors_event_t); 52549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gravity].sensor = ID_GR; 52649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; 52749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 52849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 52949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gyro].version = sizeof(sensors_event_t); 53049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gyro].sensor = ID_GY; 53149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; 53249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 53349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 53449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawGyro].version = sizeof(sensors_event_t); 53549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawGyro].sensor = ID_RG; 53649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; 53749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 53849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 53949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); 54049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].sensor = ID_A; 54149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; 54249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].acceleration.status 54349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_ACCURACY_HIGH; 54449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Invensense compass calibration */ 54649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[MagneticField].version = sizeof(sensors_event_t); 54749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[MagneticField].sensor = ID_M; 54849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; 54949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[MagneticField].magnetic.status = 55049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSOR_STATUS_ACCURACY_HIGH; 55149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 55249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t); 55349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawMagneticField].sensor = ID_RM; 55449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; 55549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[RawMagneticField].magnetic.status = 55649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSOR_STATUS_ACCURACY_HIGH; 55749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 55849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Pressure].version = sizeof(sensors_event_t); 55949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Pressure].sensor = ID_PS; 560cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPendingEvents[Pressure].type = SENSOR_TYPE_PRESSURE; 56149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Pressure].magnetic.status = 56249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow SENSOR_STATUS_ACCURACY_HIGH; 56349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Orientation].version = sizeof(sensors_event_t); 56549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Orientation].sensor = ID_O; 56649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; 56749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Orientation].orientation.status 56849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_ACCURACY_HIGH; 56949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t); 57149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV; 57249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GeomagneticRotationVector].type 57349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR; 57449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[GeomagneticRotationVector].acceleration.status 57549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_ACCURACY_HIGH; 57649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef KLP 57849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[RotationVector] = &MPLSensor::rvHandler; 57949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 58049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[RotationVector] = &MPLSensor::grvHandler; 58149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 58249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[GameRotationVector] = &MPLSensor::grvHandler; 58349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[LinearAccel] = &MPLSensor::laHandler; 58449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[Gravity] = &MPLSensor::gravHandler; 58549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef KLP 58649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[Gyro] = &MPLSensor::gyroHandler; 58749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 58849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[Gyro] = &MPLSensor::rawGyroHandler; 58949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 59049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[RawGyro] = &MPLSensor::rawGyroHandler; 59149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[Accelerometer] = &MPLSensor::accelHandler; 59249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef KLP 59349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[MagneticField] = &MPLSensor::compassHandler; 59449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 59549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[MagneticField] = &MPLSensor::rawCompassHandler; 59649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 59749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[RawMagneticField] = &MPLSensor::rawCompassHandler; 59849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[Orientation] = &MPLSensor::orienHandler; 59949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler; 60049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHandlers[Pressure] = &MPLSensor::psHandler; 60149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 60249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = 0; i < NumSensors; i++) { 60349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDelays[i] = 1000000000LL; 60449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mBatchDelays[i] = 1000000000LL; 605cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mBatchTimeouts[i] = 100000000000LL; 60649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 60749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* initialize Compass Bias */ 6094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memset(mCompassBias, 0, sizeof(mCompassBias)); 61049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* initialize Factory Accel Bias */ 6124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias)); 61349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* initialize Gyro Bias */ 6154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memset(mGyroBias, 0, sizeof(mGyroBias)); 6164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memset(mGyroChipBias, 0, sizeof(mGyroChipBias)); 61749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 61849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* load calibration file from /data/inv_cal_data.bin */ 6194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rv = inv_load_calibration(); 62049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(rv == INV_SUCCESS) { 621cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded"); 6224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* Get initial values */ 6234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getCompassBias(); 6244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getGyroBias(); 625cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mGyroBiasAvailable) { 626cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setGyroBias(); 627cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getAccelBias(); 62949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getFactoryGyroBias(); 6304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mFactoryGyroBiasAvailable) { 6314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow setFactoryGyroBias(); 6324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 633cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getFactoryAccelBias(); 634cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mFactoryAccelBiasAvailable) { 635cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setFactoryAccelBias(); 636cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 63749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 63849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 63949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); 64049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 64149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* takes external accel calibration load workflow */ 64249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if( m_pt2AccelCalLoadFunc != NULL) { 64349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long accel_offset[3]; 64449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long tmp_offset[3]; 64549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int result = m_pt2AccelCalLoadFunc(accel_offset); 64649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(result) 64749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", 64849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result); 64949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 65049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGW("HAL:Vendor accelerometer calibration file successfully " 65149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "loaded"); 652cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_mpl_accel_bias(tmp_offset, NULL); 65349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 65449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:Original accel offset, %ld, %ld, %ld\n", 65549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tmp_offset[0], tmp_offset[1], tmp_offset[2]); 656cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_accel_bias_mask(accel_offset, mAccelAccuracy,4); 657cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_mpl_accel_bias(tmp_offset, NULL); 65849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n", 65949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tmp_offset[0], tmp_offset[1], tmp_offset[2]); 66049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 66149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 6624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* end of external accel calibration load workflow */ 66349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 66449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* disable driver master enable the first sensor goes on */ 66549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(0); 66649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableGyro(0); 667cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro enableLowPowerAccel(0); 66849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableAccel(0); 66949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableCompass(0,0); 67049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enablePressure(0); 6714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enableBatch(0); 67249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 67349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isLowPowerQuatEnabled()) { 67449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(0); 67549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 67649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 67749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isDmpDisplayOrientationOn()) { 67849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // open DMP Orient Fd 67949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow openDmpOrientFd(); 68049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableDmpOrientation(!isDmpScreenAutoRotationEnabled()); 68149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 68249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 68349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 68449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::enable_iio_sysfs(void) 68549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 68649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 68749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow char iio_device_node[MAX_CHIP_ID_LEN]; 68949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *tempFp = NULL; 69049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 69149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", 69249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mpu.in_timestamp_en, getTimestamp()); 69349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Either fopen()/open() are okay for sysfs access 69449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // developers could choose what they want 69549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // with fopen(), the benefit is that fprintf()/fscanf() are available 69649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tempFp = fopen(mpu.in_timestamp_en, "w"); 69749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (tempFp == NULL) { 69849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open timestamp enable"); 69949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 70049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) { 70149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not enable timestamp"); 70249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 70349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 70449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 70549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 70649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp()); 70749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tempFp = fopen(mpu.buffer_length, "w"); 70849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (tempFp == NULL) { 70949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open buffer length"); 71049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 71149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0 || fclose(tempFp) < 0) { 71249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not write buffer length"); 71349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 71449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 71549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 716cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 717cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1, mpu.chip_enable, getTimestamp()); 718cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempFp = fopen(mpu.chip_enable, "w"); 719cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (tempFp == NULL) { 720cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:could not open chip enable"); 721cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 722cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fprintf(tempFp, "%d", 1) < 0 || fclose(tempFp) < 0) { 723cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:could not write chip enable"); 724cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 725cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 726cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 72749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_iio_device_node(iio_device_node); 72849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow iio_fd = open(iio_device_node, O_RDONLY); 72949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (iio_fd < 0) { 73049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open iio device node"); 73149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 732cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd); 73349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 73449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 73549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 73649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_constructor_init(void) 73749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 73849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 73949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 74049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result = inv_init_mpl(); 74149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 74249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:inv_init_mpl() failed"); 74349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 74449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 74549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_constructor_default_enable(); 74649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_start_mpl(); 74749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 74849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:inv_start_mpl() failed"); 74949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 75049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 75149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 75249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 75349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 75449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 75549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 75649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_constructor_default_enable(void) 75749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 75849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 75949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 76049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t result; 76149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 76249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/******************************************************************************* 76349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 76449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow******************************************************************************** 76549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 76649ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowThe InvenSense binary file (libmplmpu.so) is subject to Google's standard terms 76749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowand conditions as accepted in the click-through agreement required to download 76849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowthis library. 76949ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowThe library includes, but is not limited to the following function calls: 77049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_enable_quaternion(). 77149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 77249ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED. 77349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 77449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow******************************************************************************** 77549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 77649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*******************************************************************************/ 77749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 77849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_quaternion(); 77949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 78049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Cannot enable quaternion\n"); 78149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 78249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 78349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 78449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_in_use_auto_calibration(); 78549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 78649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 78749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 78849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 78949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_fast_nomot(); 79049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 79149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 79249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 79349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 79449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_gyro_tc(); 79549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 79649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 79749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 79849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 79949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_hal_outputs(); 80049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 80149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 80249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 80349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 80449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!mCompassSensor->providesCalibration()) { 80549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Invensense compass calibration */ 806cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Invensense vector compass cal enabled"); 80749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_vector_compass_cal(); 80849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 80949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 81049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 81149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 81249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mMplFeatureActiveMask |= INV_COMPASS_CAL; 81349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 81449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // specify MPL's trust weight, used by compass algorithms 81549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_vector_compass_cal_sensitivity(3); 81649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 81749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* disabled by default 81849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_compass_bias_w_gyro(); 81949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 82049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 82149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 82249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 82349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 82449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 82549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_heading_from_gyro(); 82649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 82749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 82849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 82949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 83049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 83149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_magnetic_disturbance(); 83249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 83349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 83449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 83549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 83649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //inv_enable_magnetic_disturbance_logging(); 83749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 83849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 83949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_9x_sensor_fusion(); 84049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 84149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 84249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 84349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 84449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 9x sensor fusion enables Compass fit 84549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mMplFeatureActiveMask |= INV_COMPASS_FIT; 84649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 84749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 84849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow result = inv_enable_no_gyro_fusion(); 84949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (result) { 85049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOG_RESULT_LOCATION(result); 85149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 85249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 85349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 85449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return result; 85549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 85649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 85749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* TODO: create function pointers to calculate scale */ 85849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::inv_set_device_properties(void) 85949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 86049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 86149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 86249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short orient; 86349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 86449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_sensors_orientation(); 86549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 86649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE); 86749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE); 86849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 86949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* gyro setup */ 87049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow orient = inv_orientation_matrix_to_scalar(mGyroOrientation); 87149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15); 87249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15); 87349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 87449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* accel setup */ 87549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow orient = inv_orientation_matrix_to_scalar(mAccelOrientation); 87649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* use for third party accel input subsystem driver 87749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_accel_orientation_and_scale(orient, 1LL << 22); 87849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 87949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15); 88049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(EXTRA_VERBOSE, 88149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15); 88249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 88349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* compass setup */ 88449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow signed char orientMtx[9]; 88549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->getOrientationMatrix(orientMtx); 88649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow orient = 88749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_orientation_matrix_to_scalar(orientMtx); 88849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long sensitivity; 88949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensitivity = mCompassSensor->getSensitivity(); 89049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_orientation_and_scale(orient, sensitivity); 89149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassScale = sensitivity; 89249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(EXTRA_VERBOSE, 89349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL: Set MPL Compass Scale %ld", mCompassScale); 89449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 89549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 89649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::loadDMP(void) 89749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 898cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 899cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 90049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res, fd; 90149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *fptr; 90249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 90349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isMpuNonDmp()) { 90449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //DMP support only for MPU6xxx/9xxx currently 90549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 90649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 90749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 90849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* load DMP firmware */ 90949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, 91049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp()); 91149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fd = open(mpu.firmware_loaded, O_RDONLY); 91249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fd < 0) { 91349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:could not open dmp state"); 91449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 91549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(inv_read_dmp_state(fd) == 0) { 91649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware); 91749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fptr = fopen(mpu.dmp_firmware, "w"); 9184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(fptr == NULL) { 919cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:could not open dmp_firmware"); 92049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 9214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (inv_load_dmp(fptr) < 0 || fclose(fptr) < 0) { 92249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:load DMP failed"); 92349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 92449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); 925cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 92649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 92749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 928cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded"); 92949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 93049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 93149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // onDmp(1); //Can't enable here. See note onDmp() 93349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 93449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 93549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::inv_get_sensors_orientation(void) 93649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 937cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 938cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 93949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *fptr; 94049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 94149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // get gyro orientation 94249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, 94349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp()); 94449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fptr = fopen(mpu.gyro_orient, "r"); 94549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (fptr != NULL) { 94649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int om[9]; 947cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 94849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 949cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { 950cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Could not read gyro mounting matrix"); 951cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 952cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, 953cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:gyro mounting matrix: " 954cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "%+d %+d %+d %+d %+d %+d %+d %+d %+d", 955cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); 95649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 957cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[0] = om[0]; 958cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[1] = om[1]; 959cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[2] = om[2]; 960cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[3] = om[3]; 961cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[4] = om[4]; 962cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[5] = om[5]; 963cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[6] = om[6]; 964cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[7] = om[7]; 965cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mGyroOrientation[8] = om[8]; 966cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 96749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 96849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 96949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // get accel orientation 97049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, 97149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp()); 97249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fptr = fopen(mpu.accel_orient, "r"); 97349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (fptr != NULL) { 97449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int om[9]; 975cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 976cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 977cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro &om[6], &om[7], &om[8]) < 0 || fclose(fptr) < 0) { 978cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:could not read accel mounting matrix"); 979cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 980cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, 981cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:accel mounting matrix: " 982cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "%+d %+d %+d %+d %+d %+d %+d %+d %+d", 983cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); 98449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 985cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[0] = om[0]; 986cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[1] = om[1]; 987cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[2] = om[2]; 988cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[3] = om[3]; 989cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[4] = om[4]; 990cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[5] = om[5]; 991cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[6] = om[6]; 992cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[7] = om[7]; 993cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelOrientation[8] = om[8]; 994cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 99549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 99649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 99749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 99849ea3e26ca3c6a779e527a0322e49a663333350aRosa ChowMPLSensor::~MPLSensor() 99949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 100049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 100149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 100249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Close open fds */ 100349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (iio_fd > 0) 100449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(iio_fd); 100549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if( accel_fd > 0 ) 100649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(accel_fd ); 100749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_temperature_fd > 0) 100849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(gyro_temperature_fd); 100949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sysfs_names_ptr) 101049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow free(sysfs_names_ptr); 101149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 101249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow closeDmpOrientFd(); 101349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 101449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accel_x_dmp_bias_fd > 0) { 101549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(accel_x_dmp_bias_fd); 101649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 101749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accel_y_dmp_bias_fd > 0) { 101849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(accel_y_dmp_bias_fd); 101949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 102049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accel_z_dmp_bias_fd > 0) { 102149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(accel_z_dmp_bias_fd); 102249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 102349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 102449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_x_dmp_bias_fd > 0) { 102549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(gyro_x_dmp_bias_fd); 102649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 102749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_y_dmp_bias_fd > 0) { 102849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(gyro_y_dmp_bias_fd); 102949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 103049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_z_dmp_bias_fd > 0) { 103149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(gyro_z_dmp_bias_fd); 103249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 103349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 103449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_x_offset_fd > 0) { 103549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(gyro_x_dmp_bias_fd); 103649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 103749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_y_offset_fd > 0) { 103849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(gyro_y_offset_fd); 103949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 104049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (gyro_z_offset_fd > 0) { 104149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(accel_z_offset_fd); 104249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 104349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 104449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Turn off Gyro master enable */ 104549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* A workaround until driver handles it */ 104649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: Turn off and close all sensors */ 10474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 1048cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 0, mpu.master_enable, getTimestamp()); 1049cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro write_sysfs_int(mpu.master_enable, 0); 105049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 105149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 105249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_turn_off_data_logging(); 1053cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fclose(logfile) < 0) { 1054cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("cannot close debug log file"); 1055cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 105649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 105749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 105849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 105949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) 106049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define RGY_ENABLED ((1 << ID_RG) & enabled_sensors) 106149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define A_ENABLED ((1 << ID_A) & enabled_sensors) 106249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define M_ENABLED ((1 << ID_M) & enabled_sensors) 106349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define RM_ENABLED ((1 << ID_RM) & enabled_sensors) 106449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define PS_ENABLED ((1 << ID_PS) & enabled_sensors) 106549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define O_ENABLED ((1 << ID_O) & enabled_sensors) 106649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) 106749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) 106849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) 106949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors) 107049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors) 107149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 107249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* TODO: this step is optional, remove? */ 107349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::setGyroInitialState(void) 107449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 107549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 107649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 107749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 107849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 107949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 10804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow HW_GYRO_RATE_HZ, mpu.gyro_rate, getTimestamp()); 10814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int fd = open(mpu.gyro_rate, O_RDWR); 108249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = errno; 108349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fd < 0) { 108449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:open of %s failed with '%s' (%d)", 10854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mpu.gyro_rate, strerror(res), res); 108649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 108749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 108849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ); 108949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 109049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:write_attribute_sensor : error writing %s with %d", 10914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mpu.gyro_rate, HW_GYRO_RATE_HZ); 109249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 109349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 109449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 109549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Setting LPF is deprecated 109649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 109749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 109849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 109949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 110049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* this applies to BMA250 Input Subsystem Driver only */ 110149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::setAccelInitialState() 110249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 110349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 110449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 110549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct input_absinfo absinfo_x; 110649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct input_absinfo absinfo_y; 110749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct input_absinfo absinfo_z; 110849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float value; 110949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) && 111049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) && 111149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) { 111249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow value = absinfo_x.value; 111349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X; 111449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow value = absinfo_y.value; 111549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y; 111649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow value = absinfo_z.value; 111749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z; 111849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //mHasPendingEvent = true; 111949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 112049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 112149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 112249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 112349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::onDmp(int en) 112449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 112549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 112649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 112749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = -1; 112849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status; 112949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpOn = en; 113049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 113149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Sequence to enable DMP 113249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //1. Load DMP image if not already loaded 113349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //2. Either Gyro or Accel must be enabled/configured before next step 113449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //3. Enable DMP 113549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 113649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 113749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mpu.firmware_loaded, getTimestamp()); 113849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){ 113949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't get firmware_loaded status"); 114049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (status == 1) { 114149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Write only if curr DMP state <> request 114249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 114349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mpu.dmp_on, getTimestamp()); 114449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (read_sysfs_int(mpu.dmp_on, &status) < 0) { 114549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't read DMP state"); 114649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (status != en) { 114749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 114849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_on, getTimestamp()); 114949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_on, en) < 0) { 115049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write dmp_on"); 115149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 115249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpOn = en; 1153cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = 0; //Indicate write successful 115449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(!en) { 115549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow setAccelBias(); 115649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 115749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 115849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Enable DMP interrupt 115949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 116049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_int_on, getTimestamp()); 116149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { 116249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't en/dis DMP interrupt"); 116349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 116449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 116549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP event interrupt if needed 116649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 116749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 116849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_event_int_on, getTimestamp()); 116949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 117049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 117149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 117249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 117349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 117449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 117549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpOn = en; 1176cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = 0; //DMP already set as requested 117749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(!en) { 117849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow setAccelBias(); 117949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 118049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 118149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 118249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR No DMP image"); 118349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 118449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 118549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 118649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 11874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* called when batch and hw sensor enabled*/ 11884a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::enablePedIndicator(int en) 11894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 11904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow VFUNC_LOG; 11914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 11924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int res = 0; 11934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (en) { 11944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { 11954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable DMP Pedometer Interrupt 11964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 11974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 0, mpu.pedometer_int_on, getTimestamp()); 11984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { 11994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); 12004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; // indicate an err 12014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 12024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1206cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Toggling step indicator to %d", en); 12074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.step_indicator_on, getTimestamp()); 12094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { 12104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; 12114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't write to DMP step_indicator_on"); 12124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 12144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 12154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12164a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::checkPedStandaloneEnabled(void) 12174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 1218cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 12194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0); 12204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 12214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* This feature is only used in batch mode */ 12234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* Stand-alone Step Detector */ 12244a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::enablePedStandalone(int en) 12254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 12264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow VFUNC_LOG; 12274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!en) { 12294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedStandaloneData(0); 12304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE; 12314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mFeatureActiveMask == 0) { 12324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow onDmp(0); 12334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) { 12344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Re-enable DMP Pedometer Interrupt 12354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1, mpu.pedometer_int_on, getTimestamp()); 12374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) { 12384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); 12394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return (-1); 12404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable data interrupt if no continuous data 12424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mEnabled == 0) { 12434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1, mpu.dmp_event_int_on, getTimestamp()); 12454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) { 12464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 12474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return (-1); 12484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 1251cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone disabled"); 12524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 12534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) { 12544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable Ped Standalone"); 12554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 12564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFeatureActiveMask |= INV_DMP_PED_STANDALONE; 12574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable DMP Pedometer Interrupt 12584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 0, mpu.pedometer_int_on, getTimestamp()); 12604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { 12614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't disable Android Pedometer Interrupt"); 12624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return (-1); 12634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Enable Data Interrupt 12654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 0, mpu.dmp_event_int_on, getTimestamp()); 12674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { 12684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 12694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return (-1); 12704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 1271cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone enabled"); 12724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return 0; 12754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 12764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12774a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor:: enablePedStandaloneData(int en) 12784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 12794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow VFUNC_LOG; 12804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int res = 0; 12824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // Enable DMP Ped standalone 12844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.step_detector_on, getTimestamp()); 12864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.step_detector_on, en) < 0) { 12874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't write DMP step_detector_on"); 12884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; //Indicate an err 12894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // Disable DMP Step indicator 12924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 12934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.step_indicator_on, getTimestamp()); 12944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { 12954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't write DMP step_indicator_on"); 12964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; //Indicate an err 12974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 12984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 12994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!en) { 1300cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped standalone"); 13014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable Accel if no sensor needs it 13024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & DMP_FEATURE_MASK) 13034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow && (!(mLocalSensorMask & mMasterSensorMask 13044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow & INV_THREE_AXIS_ACCEL))) { 13054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = enableAccel(0); 13064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 13074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 13084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 13094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & DMP_FEATURE_MASK) 13104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow && (!(mLocalSensorMask & mMasterSensorMask 13114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow & INV_THREE_AXIS_GYRO))) { 13124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = enableGyro(0); 13134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 13144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 13154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 13164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 1317cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone"); 13184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // enable accel engine 13194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = enableAccel(1); 13204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 13214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 13224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); 13234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // disable accel FIFO 13244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) { 13254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = turnOffAccelFifo(); 13264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 13274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 13284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 13294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 13304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 13314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 13324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 13334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 133449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::checkPedQuatEnabled(void) 133549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 1336cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 133749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0); 133849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 133949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 134049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* This feature is only used in batch mode */ 134149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* Step Detector && Game Rotation Vector */ 134249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enablePedQuaternion(int en) 134349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 134449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 134549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 134649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 134749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enablePedQuaternionData(0); 134849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION; 134949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask == 0) { 135049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow onDmp(0); 135149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) { 135249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Re-enable DMP Pedometer Interrupt 135349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 135449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1, mpu.pedometer_int_on, getTimestamp()); 135549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) { 135649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); 135749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (-1); 135849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 135949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Disable data interrupt if no continuous data 136049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mEnabled == 0) { 136149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 136249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1, mpu.dmp_event_int_on, getTimestamp()); 136349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 136449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 136549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (-1); 136649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 136749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 136849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 1369cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat disabled"); 137049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 137149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) { 137249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable Ped Quaternion"); 137349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 137449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask |= INV_DMP_PED_QUATERNION; 137549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Disable DMP Pedometer Interrupt 137649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 137749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.pedometer_int_on, getTimestamp()); 137849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { 137949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't disable Android Pedometer Interrupt"); 138049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (-1); 138149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 138249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Enable Data Interrupt 138349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 138449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.dmp_event_int_on, getTimestamp()); 138549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { 138649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 138749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (-1); 138849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 1389cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat enabled"); 139049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 139149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 139249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 139349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 139449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 139549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enablePedQuaternionData(int en) 139649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 139749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 139849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 139949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 140049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 140149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Enable DMP quaternion 140249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 140349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.ped_q_on, getTimestamp()); 140449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.ped_q_on, en) < 0) { 140549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write DMP ped_q_on"); 140649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; //Indicate an err 140749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 140849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 14094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // toggle DMP step indicator 14104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /*LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 14114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.step_indicator_on, getTimestamp()); 14124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { 14134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't write DMP step_indicator_on"); 14144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; //Indicate an err 14154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow }*/ 14164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 141749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 1418cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped quat"); 14194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable Accel if no sensor needs it 14204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & DMP_FEATURE_MASK) 142149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (!(mLocalSensorMask & mMasterSensorMask 142249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_ACCEL))) { 142349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(0); 142449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 142549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 14264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 14274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & DMP_FEATURE_MASK) 142849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (!(mLocalSensorMask & mMasterSensorMask 142949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_GYRO))) { 14304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = enableGyro(0); 14314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 14324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 14334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 1434cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mFeatureActiveMask & INV_DMP_QUATERNION) { 14354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_sysfs_int(mpu.gyro_fifo_enable, 1); 14364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res += write_sysfs_int(mpu.accel_fifo_enable, 1); 14374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 14384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 1439cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1440cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //LOGV_IF(ENG_VERBOSE, "before mLocalSensorMask=0x%lx", mLocalSensorMask); 1441cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // reset global mask for buildMpuEvent() 1442cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mEnabled & (1 << GameRotationVector)) { 1443cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLocalSensorMask |= INV_THREE_AXIS_GYRO; 1444cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 1445cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (mEnabled & (1 << Accelerometer)) { 1446cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 1447cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if ((mEnabled & ( 1 << Gyro)) || 1448cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (mEnabled & (1 << RawGyro))) { 1449cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLocalSensorMask |= INV_THREE_AXIS_GYRO; 1450cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1451cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); 145249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 145349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat"); 145449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable accel engine 145549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(1); 145649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 145749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 145849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 145949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable gyro engine 146049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableGyro(1); 146149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 146249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 14634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); 146449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel FIFO 14654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) || 14664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow !(mBatchEnabled & (1 << Accelerometer))) { 146749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = turnOffAccelFifo(); 146849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 14694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 14704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; 147149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 147249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 147349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable gyro FIFO 14744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) || 14754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) { 147649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = turnOffGyroFifo(); 147749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 14784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 14794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 148049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 148149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 148249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 148349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 148449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 148549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 148649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::check6AxisQuatEnabled(void) 148749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 1488cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 148949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0); 149049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 149149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 14924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* This is used for batch mode only */ 14934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* GRV is batched but not along with ped */ 149449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enable6AxisQuaternion(int en) 149549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 149649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 149749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 149849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 149949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enable6AxisQuaternionData(0); 150049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION; 150149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask == 0) { 150249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow onDmp(0); 150349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 1504cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:6 Axis Quat disabled"); 150549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 150649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) { 150749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable 6 Axis Quaternion"); 150849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 150949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION; 151049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled"); 151149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 151249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 151349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 151449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 151549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 151649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enable6AxisQuaternionData(int en) 151749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 151849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 151949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1520cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int res = 0; 1521cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 152249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Enable DMP quaternion 152349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 152449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.six_axis_q_on, getTimestamp()); 152549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) { 15264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't write DMP six_axis_q_on"); 152749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; //Indicate an err 152849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 152949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 153049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 153149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off"); 1532cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_quaternion_sensor_was_turned_off(); 153349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mFeatureActiveMask & DMP_FEATURE_MASK) 153449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (!(mLocalSensorMask & mMasterSensorMask 153549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_ACCEL))) { 153649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(0); 153749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 153849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 153949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 154049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mFeatureActiveMask & DMP_FEATURE_MASK) 154149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (!(mLocalSensorMask & mMasterSensorMask 154249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_GYRO))) { 154349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableGyro(0); 154449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 154549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 154649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 154749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask & INV_DMP_QUATERNION) { 15484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 15494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1, mpu.gyro_fifo_enable, getTimestamp()); 155049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_sysfs_int(mpu.gyro_fifo_enable, 1); 15514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 15524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1, mpu.accel_fifo_enable, getTimestamp()); 155349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res += write_sysfs_int(mpu.accel_fifo_enable, 1); 155449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 155549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 155649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 1557cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, " k=0x%lx", mLocalSensorMask); 15584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // reset global mask for buildMpuEvent() 15594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mEnabled & (1 << GameRotationVector)) { 15604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { 15614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_THREE_AXIS_GYRO; 15624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 1563cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = write_sysfs_int(mpu.gyro_fifo_enable, 1); 1564cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res += write_sysfs_int(mpu.accel_fifo_enable, 1); 1565cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (res < 0) 1566cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return res; 15674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 15684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if (mEnabled & (1 << Accelerometer)) { 15694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 15704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if ((mEnabled & ( 1 << Gyro)) || 15714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mEnabled & (1 << RawGyro))) { 15724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_THREE_AXIS_GYRO; 15734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 1574cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); 157549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 157649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat"); 15774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mEnabled & ( 1 << GameRotationVector)) { 15784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // enable accel engine 15794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = enableAccel(1); 158049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 158149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 15824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 15834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // enable gyro engine 15844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = enableGyro(1); 158549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 158649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 15874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask); 15884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) || 15894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (!(mBatchEnabled & (1 << Accelerometer)) || 15904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (!(mEnabled & (1 << Accelerometer))))) { 15914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = turnOffAccelFifo(); 15924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 15934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 15944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; 159549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 15964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 15974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) || 15984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (!(mBatchEnabled & (1 << Gyro)) || 15994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (!(mEnabled & (1 << Gyro))))) { 16004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mBatchEnabled & (1 << RawGyro)) || 16014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (!(mEnabled & (1 << RawGyro)))) { 16024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = turnOffGyroFifo(); 16034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (res < 0) 16044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 16054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 16064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 160749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 1608cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "after: mLocalSensorMask=0x%lx", mLocalSensorMask); 160949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 161049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 161149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 161249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 161349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 161449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1615cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/* this is for batch mode only */ 1616cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::checkLPQRateSupported(void) 1617cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 1618cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 1619cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return ((mDelays[GameRotationVector] <= RATE_200HZ) ? 0 :1); 1620cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 1621cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 162249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::checkLPQuaternion(void) 162349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 1624cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 162549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); 162649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 162749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 162849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableLPQuaternion(int en) 162949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 163049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 163149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 163249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 163349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableQuaternionData(0); 163449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask &= ~INV_DMP_QUATERNION; 1635cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mFeatureActiveMask == 0) { 1636cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro onDmp(0); 1637cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 1638cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:LP Quat disabled"); 163949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 164049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (enableQuaternionData(1) < 0 || onDmp(1) < 0) { 164149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable LP Quaternion"); 164249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 164349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask |= INV_DMP_QUATERNION; 1644cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:LP Quat enabled"); 164549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 164649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 164749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 164849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 164949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 165049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableQuaternionData(int en) 165149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 165249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 165349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1654cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int res = 0; 1655cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 165649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Enable DMP quaternion 165749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 165849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.three_axis_q_on, getTimestamp()); 165949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) { 166049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write DMP three_axis_q__on"); 1661cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = -1; //Indicates an err 166249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 166349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 166449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 1665cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:DMP quaternion data was turned off"); 166649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_quaternion_sensor_was_turned_off(); 166749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 1668cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Enabling three axis quat"); 166949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 167049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 167149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 167249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 167349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 167449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableDmpPedometer(int en, int interruptMode) 167549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 167649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 167749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 167849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int enabled_sensors = mEnabled; 167949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 168049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isMpuNonDmp()) 168149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 168249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 168349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // reset master enable 168449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 168549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 168649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 168749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 168849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 168949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (en == 1) { 169049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Enable DMP Pedometer Function 169149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 169249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.pedometer_on, getTimestamp()); 169349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.pedometer_on, en) < 0) { 169449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable Android Pedometer"); 169549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; // indicate an err 169649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 169749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 169849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 16994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (interruptMode || (mFeatureActiveMask & INV_DMP_PEDOMETER)) { 17004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Enable DMP Pedometer Interrupt 17014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 17024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.pedometer_int_on, getTimestamp()); 17034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) { 17044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); 17054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; // indicate an err 17064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 17074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 170849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 170949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable DMP 171049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(1); 171149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 171249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 17134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 171449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 171549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set DMP rate to 200Hz 171649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 171749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 200, mpu.accel_fifo_rate, getTimestamp()); 171849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { 171949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 17204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set rate to 200Hz"); 172149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 172249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 172349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 172449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable accel engine 172549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(1); 172649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 172749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 172849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 172949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 173049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel FIFO 173149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { 173249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = turnOffAccelFifo(); 173349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 173449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 173549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 173649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 173749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable data interrupt 17384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //if (!batchPed && enabled_sensors == 0) { 173949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (enabled_sensors == 0) { 174049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 174149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_event_int_on, getTimestamp()); 174249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 174349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 174449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 174549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 174649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 17474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (interruptMode) { 174849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask |= INV_DMP_PEDOMETER; 17494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 17504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow else { 17514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP; 1752cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mStepCountPollTime = 1000; 17534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 17544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 175549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow clock_gettime(CLOCK_MONOTONIC, &mt_pre); 17564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 17574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (interruptMode) { 17584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFeatureActiveMask &= ~INV_DMP_PEDOMETER; 17594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 17604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow else { 17614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP; 1762cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mStepCountPollTime = -1; 17634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 17644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 17654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* if neither step detector or step count is on */ 17664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) { 17674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable DMP Pedometer Function 17684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 17694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.pedometer_on, getTimestamp()); 17704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.pedometer_on, en) < 0) { 17714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable Android Pedometer"); 17724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; // indicate an err 17734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 17744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 17754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 177649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mFeatureActiveMask == 0 ) { 177849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP 177949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(0); 178049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 178149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 178249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 17834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 17844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // disable accel engine 178549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask 178649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_ACCEL)) { 178749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(0); 178849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 178949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 179049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 179149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 179249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 17934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 17944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* if feature is not step detector */ 17954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) { 17964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //Disable DMP Pedometer Interrupt 17974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 17984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.pedometer_int_on, getTimestamp()); 17994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) { 18004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); 18014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = -1; // indicate an err 18024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 18034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 18044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 18054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 180649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //enable data interrupts if applicable 180749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (enabled_sensors) { 180849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 180949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_event_int_on, getTimestamp()); 181049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 181149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 181249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 181349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 181449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 181549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 18164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 181749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(en || enabled_sensors || mFeatureActiveMask) { 181849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 181949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 182049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 182149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 182249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 182349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::masterEnable(int en) 182449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 182549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 182649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 182749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 182849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 1829cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro en, mpu.master_enable, getTimestamp()); 1830cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = write_sysfs_int(mpu.master_enable, en); 183149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 183249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 183349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 183449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableGyro(int en) 183549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 183649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 183749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 183849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 183949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 184049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* need to also turn on/off the master enable */ 18414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 18424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.gyro_enable, getTimestamp()); 184349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_sysfs_int(mpu.gyro_enable, en); 18444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 18454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.gyro_fifo_enable, getTimestamp()); 18464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res += write_sysfs_int(mpu.gyro_fifo_enable, en); 184749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 184849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 184949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); 185049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_gyro_was_turned_off(); 185149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 185249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 185349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 185449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 185549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1856cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::enableLowPowerAccel(int en) 1857cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 1858cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 1859cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1860cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int res; 1861cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1862cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* need to also turn on/off the master enable */ 1863cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = write_sysfs_int(mpu.motion_lpa_on, en); 1864cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 1865cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro en, mpu.motion_lpa_on, getTimestamp()); 1866cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return res; 1867cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 1868cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 186949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableAccel(int en) 187049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 187149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 187249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 187349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res; 187449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 187549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* need to also turn on/off the master enable */ 18764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 18774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.accel_enable, getTimestamp()); 187849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_sysfs_int(mpu.accel_enable, en); 18794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 18804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow en, mpu.accel_fifo_enable, getTimestamp()); 18814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res += write_sysfs_int(mpu.accel_fifo_enable, en); 188249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 188349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) { 188449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); 188549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_accel_was_turned_off(); 188649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 188749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 188849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 188949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 189049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 189149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableCompass(int en, int rawSensorRequested) 189249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 189349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 189449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 189549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 189649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* handle ID_RM if third party compass cal is used */ 189749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (rawSensorRequested && mCompassSensor->providesCalibration()) { 189849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = mCompassSensor->enable(ID_RM, en); 189949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 190049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = mCompassSensor->enable(ID_M, en); 190149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 190249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (en == 0 || res != 0) { 190349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res); 190449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_compass_was_turned_off(); 190549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 190649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 190749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 190849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 190949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 191049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enablePressure(int en) 191149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 191249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 191349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 191449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 19154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1916cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mPressureSensor) 19178a501706d8a30295a312ccc05fba80097050204fkangsik.shin res = mPressureSensor->enable(ID_PS, en); 191849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 191949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 192049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 192149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* use this function for initialization */ 19234a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::enableBatch(int64_t timeout) 19244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 19254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow VFUNC_LOG; 19264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 19274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int res = 0; 19284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 19294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_sysfs_int(mpu.batchmode_timeout, timeout); 19304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (timeout == 0) { 19314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_sysfs_int(mpu.six_axis_q_on, 0); 19324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_sysfs_int(mpu.ped_q_on, 0); 19334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_sysfs_int(mpu.step_detector_on, 0); 19344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_sysfs_int(mpu.step_indicator_on, 0); 19354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 19364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 19374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (timeout == 0) { 19384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero"); 19394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 19404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 19414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 19424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 19434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 194449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::computeLocalSensorMask(int enabled_sensors) 194549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 194649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 194749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 194849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow do { 19494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* Invensense Pressure on secondary bus */ 19504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (PS_ENABLED) { 19514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "PS ENABLED"); 19524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_ONE_AXIS_PRESSURE; 19534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 19544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "PS DISABLED"); 19554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE; 19564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 19574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 195849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED 195949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || (GRV_ENABLED && GMRV_ENABLED)) { 196049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); 1961cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLocalSensorMask |= ALL_MPL_SENSORS_NP; 196249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 196349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 196449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 196549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GRV_ENABLED) { 196649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mBatchEnabled & (1 << GameRotationVector))) { 196749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED"); 196849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_GYRO; 196949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 19704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 19714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (GY_ENABLED || RGY_ENABLED) { 19724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "G ENABLED"); 19734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_THREE_AXIS_GYRO; 19744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 19754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "G DISABLED"); 19764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 19774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 19784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (A_ENABLED) { 19794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "A ENABLED"); 19804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 19814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 19824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "A DISABLED"); 19834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; 19844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 198549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 198649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* takes care of MAG case */ 198749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (M_ENABLED || RM_ENABLED) { 198849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(1, "M ENABLED"); 198949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 199049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 199149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(1, "M DISABLED"); 199249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; 199349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 199449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 199549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 199649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 199749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GMRV_ENABLED) { 199849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED"); 199949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 200049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 200149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 200249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* takes care of Gyro case */ 200349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GY_ENABLED || RGY_ENABLED) { 200449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(1, "G ENABLED"); 200549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_GYRO; 200649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 200749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(1, "G DISABLED"); 200849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 200949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 201049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 201149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 201249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 201349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(!A_ENABLED && !M_ENABLED && !RM_ENABLED && 201449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED && 201549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow !PS_ENABLED) { 201649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Invensense compass cal */ 201749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); 201849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask = 0; 201949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 202049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 202149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 202249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GY_ENABLED || RGY_ENABLED) { 202349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "G ENABLED"); 202449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_GYRO; 202549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 202649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "G DISABLED"); 202749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 202849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 202949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 203049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (A_ENABLED) { 203149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "A ENABLED"); 203249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_ACCEL; 203349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 203449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "A DISABLED"); 203549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; 203649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 203749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 203849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Invensense compass calibration */ 203949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (M_ENABLED || RM_ENABLED) { 204049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "M ENABLED"); 204149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 204249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 204349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "M DISABLED"); 204449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; 20454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 204649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } while (0); 204749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 204849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 204949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) 205049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 205149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 205249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 205349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t res = -1; 205449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int on = 1; 205549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int off = 0; 205649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int cal_stored = 0; 205749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 205849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Sequence to enable or disable a sensor 205949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 1. reset master enable (=0) 206049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 2. enable or disable a sensor 206149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 3. set master enable (=1) 206249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 206349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isLowPowerQuatEnabled() || 206449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | 206549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mCompassSensor->isIntegrated() << MagneticField) | 206649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mCompassSensor->isIntegrated() << RawMagneticField) | 206749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mPressureSensor->isIntegrated() << Pressure))) { 206849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 206949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* reset master enable */ 207049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 207149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 207249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 207349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 207449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 207549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2076cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", 207749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (unsigned int)sensors); 207849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 207949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (changed & ((1 << Gyro) | (1 << RawGyro))) { 2080cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - gyro %s", 2081cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (sensors & INV_THREE_AXIS_GYRO? "enable": "disable")); 208249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO)); 208349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 208449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 208549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 208649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 208749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!cal_stored && (!en && (changed & (1 << Gyro)))) { 208849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 208949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cal_stored = 1; 209049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 209149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 209249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 209349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (changed & (1 << Accelerometer)) { 2094cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - accel %s", 2095cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable")); 209649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL)); 209749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 209849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 209949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 210049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 210149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) { 210249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 210349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cal_stored = 1; 210449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 210549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 210649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 210749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) { 2108cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - compass %s", 2109cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable")); 211049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField)); 211149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 211249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 211349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 211449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 211549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!cal_stored && (!en && (changed & (1 << MagneticField)))) { 211649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 211749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cal_stored = 1; 211849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 211949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 212049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 212149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (changed & (1 << Pressure)) { 2122cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - pressure %s", 2123cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable")); 212449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE)); 212549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 212649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 212749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 212849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 212949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 213049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isLowPowerQuatEnabled()) { 213149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Enable LP Quat 213249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK) 213349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) { 2134cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: 9 axis or game rot enabled"); 213549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(changed & ((1 << Gyro) 213649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (1 << RawGyro) 213749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (1 << Accelerometer) 213849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (mCompassSensor->isIntegrated() << MagneticField) 213949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (mCompassSensor->isIntegrated() << RawMagneticField))) 214049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ) { 214149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* reset master enable */ 214249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 214349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 214449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 214549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 214649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 214749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!checkLPQuaternion()) { 214849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(1); 214949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 2150cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:LP Quat already enabled"); 215149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 215249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (checkLPQuaternion()) { 215349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(0); 215449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 215549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 215649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 215749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* apply accel/gyro bias to DMP bias */ 215849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* precondition: masterEnable(0), mGyroBiasAvailable=true */ 215949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* postcondition: bias is applied upon masterEnable(1) */ 216049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(!(sensors & INV_THREE_AXIS_GYRO)) { 216149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow setGyroBias(); 216249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 216349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(!(sensors & INV_THREE_AXIS_ACCEL)) { 216449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow setAccelBias(); 216549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 216649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 216749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* to batch or not to batch */ 216849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled); 2169cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* skip setBatch if there is no need to */ 2170cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(((int)mOldBatchEnabledMask != batchMode) || batchMode) { 2171cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setBatch(batchMode,1); 2172cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 2173cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mOldBatchEnabledMask = batchMode; 2174cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 217549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | 217649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mCompassSensor->isIntegrated() << MagneticField) | 217749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mCompassSensor->isIntegrated() << RawMagneticField) | 217849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mPressureSensor->isIntegrated() << Pressure))) { 2179cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 2180cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL DEBUG: Gyro, Accel, Compass, Pressure changes"); 218149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors & 218249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (INV_THREE_AXIS_GYRO 218349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | INV_THREE_AXIS_ACCEL 218449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()) 218549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) { 218649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled"); 2187cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 2188cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "mFeatureActiveMask=%016llx", mFeatureActiveMask); 218949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask & DMP_FEATURE_MASK) { 219049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled"); 219149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP event interrupt only (w/ data interrupt) 219249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 219349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.dmp_event_int_on, getTimestamp()); 219449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { 219549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 219649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't disable DMP event interrupt"); 219749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 219849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 219949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 2200cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%lld", mFeatureActiveMask); 2201cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "DMP_FEATURE_MASK=%d", DMP_FEATURE_MASK); 2202cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((mFeatureActiveMask & (long long)DMP_FEATURE_MASK) && 2203cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (!(mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) || 2204cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro !(mFeatureActiveMask & INV_DMP_PED_STANDALONE) || 2205cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro !(mFeatureActiveMask & INV_DMP_PED_QUATERNION))) { 220649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable DMP 220749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow onDmp(1); 220849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(on); 220949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 221049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 221149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 2212cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); 2213cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (((sensors | mLocalSensorMask) & INV_THREE_AXIS_ACCEL) == 0) { 221449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = turnOffAccelFifo(); 221549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 221649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 221749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 221849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 221949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 222049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 222149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 222249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 222349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 222449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { // all sensors idle -> reduce power 222549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors"); 222649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isDmpDisplayOrientationOn() 222749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (mDmpOrientationEnabled 222849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || !isDmpScreenAutoRotationEnabled())) { 222949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableDmpOrientation(1); 223049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 223149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 223249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!cal_stored) { 223349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 223449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cal_stored = 1; 223549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 223649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 223749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if ((changed & 223849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((!mCompassSensor->isIntegrated()) << MagneticField) || 223949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((!mCompassSensor->isIntegrated()) << RawMagneticField)) 224049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && 224149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL 224249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated())))) 224349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ) { 224449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change"); 224549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!cal_stored) { 224649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 224749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cal_stored = 1; 224849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 224949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 225049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled"); 225149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors & 225249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (INV_THREE_AXIS_GYRO 225349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | INV_THREE_AXIS_ACCEL 225449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { 225549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 225649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) 225749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 225849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 225949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 226049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 226149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 226249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 226349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 226449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* check if batch mode should be turned on or not */ 226549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor) 226649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 226749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 226849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int batchMode = 1; 2269cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFeatureActiveMask &= ~INV_DMP_BATCH_MODE; 2270cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2271cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 2272cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d", 2273cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro enableSensors, tempBatchSensor); 2274cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2275cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // handle initialization case 2276cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (enableSensors == 0 && tempBatchSensor == 0) 2277cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0; 2278cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 227949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // check for possible continuous data mode 22804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow for(int i = 0; i <= Pressure; i++) { 228149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) { 2282cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: " 2283cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "hardware sensor on continuous mode:%d", i); 228449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // if any one of the hardware sensor is in continuous data mode 228549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // turn off batch mode. 228649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 228749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 228849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) { 2289cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 2290cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:computeBatchSensorMask: hardware sensor is batch:%d", 2291cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro i); 229249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // if hardware sensor is batched, check if virtual sensor is batched 229349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((enableSensors & (1 << GameRotationVector)) 229449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && !(tempBatchSensor & (1 << GameRotationVector))) { 2295cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 2296cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:computeBatchSensorMask: but virtual sensor is not:%d", 2297cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro i); 229849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 229949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 230049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 230149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 23024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 23034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow for(int i = Orientation; i <= GeomagneticRotationVector; i++) { 23044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) { 2305cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: " 2306cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "composite sensor on continuous mode:%d", i); 23074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // if composite sensors are on but not batched 23084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // turn off batch mode. 23094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return 0; 23104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 2313cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((mFeatureActiveMask & INV_DMP_PEDOMETER) && !(tempBatchSensor & (1 << StepDetector))) { 2314cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV("HAL:computeBatchSensorMask: step detector on continuous mode."); 2315cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0; 2316cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 2317cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2318cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFeatureActiveMask |= INV_DMP_BATCH_MODE; 2319cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, 2320cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x", 2321cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro batchMode, tempBatchSensor); 232249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (batchMode && tempBatchSensor); 232349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 232449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 23254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* This function is called by enable() */ 23264a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::setBatch(int en, int toggleEnable) 232749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 232849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 232949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 233049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 233149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t wanted = 1000000000LL; 233249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t timeout = 0; 23334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int timeoutInMs = 0; 23344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int featureMask = computeBatchDataOutput(); 233549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 233649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // reset master enable 233749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 233849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 233949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 234049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 234149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 234249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (en) { 23434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* take the minimum batchmode timeout */ 2344cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int64_t timeout = 100000000000LL; 23454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t ns; 23464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow for (int i = 0; i < NumSensors; i++) { 23474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x", 23484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFeatureActiveMask, mEnabled, mBatchEnabled); 2349cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i))) || 23504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) { 23514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]); 23524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ns = mBatchTimeouts[i]; 23534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeout = (ns < timeout) ? ns : timeout; 23544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* Convert ns to millisecond */ 23574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeoutInMs = timeout / 1000000; 23584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 23594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeoutInMs = 0; 23604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 2362cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: batch timeout set to %dms", timeoutInMs); 23634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 23644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* step detector is enabled and */ 23654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* batch mode is standalone */ 23664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) && 23674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (featureMask & INV_DMP_PED_STANDALONE)) { 2368cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "ID_P only = 0x%x", mBatchEnabled); 23694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedStandalone(1); 23704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 23714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedStandalone(0); 23724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 23744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* step detector and GRV are enabled and */ 23754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* batch mode is ped q */ 23764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) && 23774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mEnabled & (1 << GameRotationVector)) && 23784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (featureMask & INV_DMP_PED_QUATERNION)) { 2379cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "ID_P and GRV or ALL = 0x%x", mBatchEnabled); 2380cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "ID_P is enabled for batching, " 2381cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "PED quat will be automatically enabled"); 23824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enableLPQuaternion(0); 23834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedQuaternion(1); 23844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if (!(featureMask & INV_DMP_PED_STANDALONE)){ 23854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedQuaternion(0); 238649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 238749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 23884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* step detector and hardware sensors enabled */ 23894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (en && (featureMask & INV_DMP_PED_INDICATOR) && 23904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ((mEnabled) || 23914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) { 23924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedIndicator(1); 23934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 23944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedIndicator(0); 23954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 23964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 23974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* GRV is enabled and */ 23984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* batch mode is 6axis q */ 23994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (en && (mEnabled & (1 << GameRotationVector)) && 24004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (featureMask & INV_DMP_6AXIS_QUATERNION)) { 2401cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "GRV = 0x%x", mBatchEnabled); 24024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enableLPQuaternion(0); 24034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enable6AxisQuaternion(1); 2404cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setInitial6QuatValue(); 24054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if (!(featureMask & INV_DMP_PED_QUATERNION)){ 2406cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "Toggle back to normal 6 axis"); 2407cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mEnabled & (1 << GameRotationVector) && checkLPQRateSupported()) { 24084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enableLPQuaternion(1); 24094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 24104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enable6AxisQuaternion(0); 24114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 24124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 241349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* write required timeout to sysfs */ 24144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 24154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeoutInMs, mpu.batchmode_timeout, getTimestamp()); 24164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) { 241749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write batchmode_timeout"); 241849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 241949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 242049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (en) { 242149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable DMP 242249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(1); 242349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 242449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 242549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 24264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 24274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // set batch rates 24284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (setBatchDataRates() < 0) { 24294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set batch data rates"); 243049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 24314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 243249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // default fifo rate to 200Hz 243349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 243449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 200, mpu.gyro_fifo_rate, getTimestamp()); 243549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) { 243649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 24374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set rate to 200Hz"); 243849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 243949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 244049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 244149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask == 0) { 244249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP 244349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(0); 244449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 244549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 244649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 2447cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* reset sensor rate */ 2448cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (resetDataRates() < 0) { 2449cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:ERR can't reset output rate back to original setting"); 2450cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 245149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 24524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* reset sensor rate */ 24534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /*if (resetDataRates() < 0) { 24544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't reset output rate back to original setting"); 24554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow }*/ 24564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 24574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (toggleEnable == 1) { 24584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mFeatureActiveMask || mEnabled) 245949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(1); 246049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 246149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 246249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 246349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 246449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* Store calibration file */ 246549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::storeCalibration(void) 246649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 2467cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 2468cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 246949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mHaveGoodMpuCal == true 247049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || mAccelAccuracy >= 2 247149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || mCompassAccuracy >= 3) { 247249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = inv_store_calibration(); 247349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res) { 247449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Cannot store calibration on file"); 247549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 247649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated"); 247749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 247849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 247949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 248049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 248149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::cbProcData(void) 248249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 2483cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 2484cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 248549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mNewData = 1; 248649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mSampleCount++; 248749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:new data"); 248849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 248949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 249049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* these handlers transform mpl data into one of the Android sensor types */ 249149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::gyroHandler(sensors_event_t* s) 249249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 249349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 249449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 249549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, 249649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->timestamp); 249749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d", 249849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); 249949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 250049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 250149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 250249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::rawGyroHandler(sensors_event_t* s) 250349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 250449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 250549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 250649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib, 250749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->gyro.status, &s->timestamp); 250849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(update) { 250949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias)); 251049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d", 251149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1], 251249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_gyro.bias[2], s->timestamp, update); 251349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 251449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->gyro.status = SENSOR_STATUS_UNRELIABLE; 251549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d", 251649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1], 251749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_gyro.uncalib[2], s->timestamp, update); 251849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 251949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 252049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 252149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::accelHandler(sensors_event_t* s) 252249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 252349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 252449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 252549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_accelerometer( 252649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->acceleration.v, &s->acceleration.status, &s->timestamp); 252749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d", 252849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], 252949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->timestamp, update); 253049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelAccuracy = s->acceleration.status; 253149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 253249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 253349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 253449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::compassHandler(sensors_event_t* s) 253549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 253649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 253749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 253849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_magnetic_field( 253949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->magnetic.v, &s->magnetic.status, &s->timestamp); 254049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d", 254149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], 254249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->timestamp, update); 254349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassAccuracy = s->magnetic.status; 254449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 254549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 254649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 254749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::rawCompassHandler(sensors_event_t* s) 254849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 254949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 255049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 2551cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //TODO: need to handle uncalib data and bias for 3rd party compass 255249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mCompassSensor->providesCalibration()) { 255349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp); 255449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 255549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 255649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib, 255749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->magnetic.status, &s->timestamp); 255849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 255949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(update) { 256049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias)); 256149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d", 256249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1], 256349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_magnetic.bias[2], s->timestamp, update); 256449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 256549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->magnetic.status = SENSOR_STATUS_UNRELIABLE; 256649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d", 256749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1], 256849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update); 256949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 257049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 257149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 257249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 257349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Rotation Vector handler. 257449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow NOTE: rotation vector does not have an accuracy or status 257549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 257649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::rvHandler(sensors_event_t* s) 257749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 257849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 257949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int8_t status; 258049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 258149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_rotation_vector(s->data, &status, 258249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->timestamp); 258349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update |= isCompassDisabled(); 258449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f- %+lld - %d", 258549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, 258649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update); 258749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 258849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 258949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 259049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 259149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 259249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Game Rotation Vector handler. 259349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow NOTE: rotation vector does not have an accuracy or status 259449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 259549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::grvHandler(sensors_event_t* s) 259649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 259749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 259849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int8_t status; 259949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 260049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status, 260149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->timestamp); 26024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /*hack*/ 26034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /*s->data[0] = mCached6AxisQuaternionData[0]; 26044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow s->data[1] = mCached6AxisQuaternionData[1]; 26054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow s->data[2] = mCached6AxisQuaternionData[2]; 26064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow update = 1;*/ 26074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 26084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 260949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f - %+lld - %d", 261049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, 261149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update); 261249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 261349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 261449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 261549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::laHandler(sensors_event_t* s) 261649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 261749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 261849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 261949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_linear_acceleration( 262049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->gyro.v, &s->gyro.status, &s->timestamp); 262149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update |= isCompassDisabled(); 262249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d", 262349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); 262449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 262549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 262649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 262749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::gravHandler(sensors_event_t* s) 262849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 262949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 263049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 263149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, 263249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->timestamp); 263349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update |= isCompassDisabled(); 263449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d", 263549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); 263649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 263749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 263849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 263949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::orienHandler(sensors_event_t* s) 264049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 264149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 264249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update; 264349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_orientation( 264449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->orientation.v, &s->orientation.status, &s->timestamp); 264549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update |= isCompassDisabled(); 264649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d", 264749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], 264849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->timestamp, update); 264949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 265049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 265149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 265249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::smHandler(sensors_event_t* s) 265349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 265449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 265549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 1; 265649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 265749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* When event is triggered, set data to 1 */ 265849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0] = 1.f; 265949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[1] = 0.f; 266049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[2] = 0.f; 266149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->acceleration.status 266249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_UNRELIABLE; 266349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 266449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Capture timestamp in HAL */ 266549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct timespec ts; 266649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow clock_gettime(CLOCK_MONOTONIC, &ts); 266749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 266849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 266949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Identify which sensor this event is for */ 267049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->version = sizeof(sensors_event_t); 267149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->sensor = ID_SM; 267249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->type = SENSOR_TYPE_SIGNIFICANT_MOTION; 267349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 267449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d", 267549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0], s->timestamp, update); 267649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update; 267749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 267849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 267949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::scHandler(sensors_event_t* s) 268049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 268149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 268249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 0; 268349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 268449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //update = readDmpPedometerEvents(s, 1); 268549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:sc data: %f - %lld - %d", 268649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0], s->timestamp, update); 268749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update < 1 ? 0 :1; 268849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 268949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 269049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::gmHandler(sensors_event_t* s) 269149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 269249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 269349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int8_t status; 269449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 0; 269549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status, 269649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow &s->timestamp); 269749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 269849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f- %+lld - %d", 269949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->timestamp, update); 270049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update < 1 ? 0 :1; 270149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 270249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 270349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 270449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::psHandler(sensors_event_t* s) 270549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 270649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 270749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int8_t status; 270849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 0; 2709cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2710cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->pressure = mCachedPressureData / 100.f; //hpa (millibar) 271149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[1] = 0; 271249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[2] = 0; 271349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->timestamp = mPressureTimestamp; 271449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->magnetic.status = 0; 2715cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro update = mPressureUpdate; 2716cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPressureUpdate = 0; 271749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 271849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d", 271949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update); 272049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return update < 1 ? 0 :1; 272149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 272249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 272349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2724cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::metaHandler(sensors_event_t* s, int flags) 2725cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 2726cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VHANDLER_LOG; 2727cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int update = 0; 2728cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2729cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* initalize SENSOR_TYPE_META_DATA */ 2730cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->version = 0; 2731cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->sensor = 0; 2732cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->reserved0 = 0; 2733cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->timestamp = 0LL; 2734cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2735cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro switch(flags) { 2736cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case META_DATA_FLUSH_COMPLETE: 2737cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro update = mFlushBatchSet; 2738cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->type = SENSOR_TYPE_META_DATA; 2739cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->meta_data.what = flags; 2740cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->meta_data.sensor = mFlushEnabled; 2741cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushBatchSet = 0; 2742cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushEnabled = -1; 2743cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(HANDLER_DATA, 2744cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:flush complete data: type=%d what=%d, " 2745cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "sensor=%d - %lld - %d", 2746cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->type, s->meta_data.what, s->meta_data.sensor, 2747cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro s->timestamp, update); 2748cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro break; 2749cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2750cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro default: 2751cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGW("HAL: Meta flags not supported"); 2752cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro break; 2753cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 2754cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 2755cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return update; 2756cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 2757cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 275849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enable(int32_t handle, int en) 275949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 276049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 276149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 276249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow android::String8 sname; 276349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int what = -1, err = 0; 27644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int batchMode = 0; 276549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 276649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow switch (handle) { 276749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SC: 276849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = StepCounter; 276949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Step Counter"; 277049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", 277149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname.string(), handle, 277249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mDmpStepCountEnabled? "en": "dis"), 277349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (en? "en" : "dis")); 277449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableDmpPedometer(en, 0); 277549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpStepCountEnabled = !!en; 277649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 277749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_P: 277849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "StepDetector"; 277949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", 278049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname.string(), handle, 278149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mDmpPedometerEnabled? "en": "dis"), 278249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (en? "en" : "dis")); 278349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableDmpPedometer(en, 1); 278449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpPedometerEnabled = !!en; 27854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled); 2786cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* skip setBatch if there is no need to */ 2787cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(((int)mOldBatchEnabledMask != batchMode) || batchMode) { 2788cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setBatch(batchMode,1); 2789cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 2790cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mOldBatchEnabledMask = batchMode; 279149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 279249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SM: 279349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Significant Motion"; 279449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", 279549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname.string(), handle, 279649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mDmpSignificantMotionEnabled? "en": "dis"), 279749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (en? "en" : "dis")); 279849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableDmpSignificantMotion(en); 279949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpSignificantMotionEnabled = !!en; 280049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 280149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SO: 280249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Screen Orientation"; 280349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", 280449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname.string(), handle, 280549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mDmpOrientationEnabled? "en": "dis"), 280649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (en? "en" : "dis")); 280749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableDmpOrientation(en && isDmpDisplayOrientationOn()); 280849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpOrientationEnabled = !!en; 280949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 281049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_A: 281149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Accelerometer; 281249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Accelerometer"; 281349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 281449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_M: 281549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = MagneticField; 281649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "MagneticField"; 281749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 281849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_RM: 281949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = RawMagneticField; 282049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "MagneticField Uncalibrated"; 282149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 282249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_O: 282349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Orientation; 282449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Orientation"; 282549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 282649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GY: 282749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Gyro; 282849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Gyro"; 282949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 283049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_RG: 283149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = RawGyro; 283249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Gyro Uncalibrated"; 283349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 283449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GR: 283549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Gravity; 283649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Gravity"; 283749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 283849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_RV: 283949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = RotationVector; 284049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "RotationVector"; 284149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 284249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GRV: 284349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = GameRotationVector; 284449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "GameRotationVector"; 284549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 284649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_LA: 284749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = LinearAccel; 284849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "LinearAccel"; 284949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 285049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GMRV: 285149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = GeomagneticRotationVector; 285249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "GeomagneticRotationVector"; 285349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 285449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_PS: 285549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Pressure; 285649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Pressure"; 285749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 285849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow default: //this takes care of all the gestures 285949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = handle; 286049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Others"; 286149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 286249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 286349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 286449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (uint32_t(what) >= NumSensors) 286549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -EINVAL; 286649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 286749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int newState = en ? 1 : 0; 286849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned long sen_mask; 286949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 287049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", 287149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname.string(), handle, 287249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((mEnabled & (1 << what)) ? "en" : "dis"), 287349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((uint32_t(newState) << what) ? "en" : "dis")); 2874cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 287549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:%s sensor state change what=%d", sname.string(), what); 287649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 287749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_lock(&mMplMutex); 287849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_lock(&mHALMutex); 287949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 288049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { 288149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t sensor_type; 288249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow short flags = newState; 288349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t lastEnabled = mEnabled, changed = 0; 288449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 288549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mEnabled &= ~(1 << what); 288649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mEnabled |= (uint32_t(flags) << what); 288749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2888cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:handle = %d", handle); 2889cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags); 289049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow computeLocalSensorMask(mEnabled); 2891cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled); 289249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled); 289349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sen_mask = mLocalSensorMask & mMasterSensorMask; 289449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mSensorMask = sen_mask; 2895cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask); 289649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 289749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow switch (what) { 289849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Gyro: 289949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case RawGyro: 290049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Accelerometer: 290149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) && 290249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) && 290349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { 290449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed |= (1 << what); 290549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 29064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) { 29074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow changed |= (1 << what); 29084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 290949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 291049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case MagneticField: 291149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case RawMagneticField: 291249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) && 291349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { 291449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed |= (1 << what); 291549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 291649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 291749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Pressure: 291849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) { 291949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed |= (1 << what); 292049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 292149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 292249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case GameRotationVector: 292349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) 292449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 292549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK)) 292649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 292749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK)) 292849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 292949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) 293049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 293149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) { 293249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = Gyro; i <= RawMagneticField; i++) { 293349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mEnabled & (1 << i))) { 293449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed |= (1 << i); 293549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 293649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 293749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 293849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 293949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 294049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Orientation: 294149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case RotationVector: 294249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case LinearAccel: 294349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Gravity: 294449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) 294549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 294649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK)) 294749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 294849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) { 294949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = Gyro; i <= RawMagneticField; i++) { 295049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mEnabled & (1 << i))) { 295149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed |= (1 << i); 295249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 295349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 295449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 295549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 295649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case GeomagneticRotationVector: 295749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!en) 295849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow storeCalibration(); 295949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK)) 296049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 296149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK)) 296249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 296349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) 296449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || 296549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) { 296649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = Accelerometer; i <= RawMagneticField; i++) { 296749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mEnabled & (1<<i))) { 296849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow changed |= (1 << i); 296949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 297049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 297149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 297249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 297349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 2974cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed); 297549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableSensors(sen_mask, flags, changed); 297649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 297749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 297849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_unlock(&mMplMutex); 297949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_unlock(&mHALMutex); 298049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 298149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef INV_PLAYBACK_DBG 298249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* apparently the logging needs to go through this sequence 298349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow to properly flush the log file */ 298449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_turn_off_data_logging(); 2985cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fclose(logfile) < 0) { 2986cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("cannot close debug log file"); 2987cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 298849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow logfile = fopen("/data/playback.bin", "ab"); 298949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (logfile) 299049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_turn_on_data_logging(logfile); 299149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 299249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 299349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return err; 299449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 299549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 299649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname) 299749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 299849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 299949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 300049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = -1; 300149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 300249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow switch (handle) { 300349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_P: 300449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = StepDetector; 300549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "StepDetector"; 300649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 300749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SC: 300849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = StepCounter; 300949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "StepCounter"; 301049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 301149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SM: 301249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = SignificantMotion; 301349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "SignificantMotion"; 301449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 301549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SO: 301649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = handle; 301749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "ScreenOrienation"; 301849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_A: 301949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Accelerometer; 302049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Accelerometer"; 302149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 302249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_M: 302349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = MagneticField; 302449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "MagneticField"; 302549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 302649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_RM: 302749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = RawMagneticField; 302849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "MagneticField Uncalibrated"; 302949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 303049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_O: 303149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Orientation; 303249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Orientation"; 303349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 303449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GY: 303549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Gyro; 303649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Gyro"; 303749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 303849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_RG: 303949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = RawGyro; 304049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Gyro Uncalibrated"; 304149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 304249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GR: 304349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Gravity; 304449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Gravity"; 304549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 304649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_RV: 304749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = RotationVector; 304849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "RotationVector"; 304949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 305049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_GRV: 305149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = GameRotationVector; 305249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "GameRotationVector"; 305349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 305449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_LA: 305549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = LinearAccel; 305649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "LinearAccel"; 305749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 305849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_PS: 305949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = Pressure; 306049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Pressure"; 306149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 306249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow default: // this takes care of all the gestures 306349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow what = handle; 306449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sname = "Others"; 306549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 306649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 306749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 306849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string()); 306949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 307049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 307149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 307249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::setDelay(int32_t handle, int64_t ns) 307349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 307449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 307549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 307649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow android::String8 sname; 307749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int what = -1; 307849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 307949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#if 0 308049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // skip the 1st call for enalbing sensors called by ICS/JB sensor service 308149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow static int counter_delay = 0; 308249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mEnabled & (1 << what))) { 308349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow counter_delay = 0; 308449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 308549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (++counter_delay == 1) { 308649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 308749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 308849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 308949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow counter_delay = 0; 309049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 309149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 309249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 309349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 309449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getHandle(handle, what, sname); 309549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (uint32_t(what) >= NumSensors) 309649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -EINVAL; 309749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 309849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (ns < 0) 309949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -EINVAL; 310049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 310149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 310249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns); 310349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 310449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // limit all rates to reasonable ones */ 310549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (ns < 5000000LL) { 310649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ns = 5000000LL; 310749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 310849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 310949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* store request rate to mDelays arrary for each sensor */ 3110cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int64_t previousDelay = mDelays[what]; 311149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDelays[what] = ns; 3112cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "storing mDelays[%d] = %lld, previousDelay = %lld", what, ns, previousDelay); 311349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 311449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow switch (what) { 311549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SC: 311649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* set limits of delivery rate of events */ 311749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mStepCountPollTime = ns; 311849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns); 311949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 312049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_P: 312149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case SignificantMotion: 312249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case ID_SO: 312349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update_delay(); 312449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 312549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Gyro: 312649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case RawGyro: 312749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Accelerometer: 3128cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // need to update delay since they are different 3129cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // resetDataRates was called earlier 3130cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //LOGV("what=%d mEnabled=%d mDelays[%d]=%lld previousDelay=%lld", 3131cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //what, mEnabled, what, mDelays[what], previousDelay); 3132cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) { 3133cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 3134cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:need to update delay due to resetDataRates"); 3135cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro break; 3136cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 313749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = Gyro; 313849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow i <= Accelerometer + mCompassSensor->isIntegrated(); 313949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow i++) { 314049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { 3141cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 314249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:ignore delay set due to sensor %d", i); 314349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 314449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 314549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 314649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 314749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 314849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case MagneticField: 314949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case RawMagneticField: 3150cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // need to update delay since they are different 3151cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // resetDataRates was called earlier 3152cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) { 3153cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 3154cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:need to update delay due to resetDataRates"); 3155cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro break; 3156cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 315749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mCompassSensor->isIntegrated() && 315849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || 315949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) || 316049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((mEnabled & (1 << Accelerometer)) && 3161cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro ns > mDelays[Accelerometer])) && 3162cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro !checkBatchEnabled()) { 3163cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* if request is slower rate, ignore request */ 3164cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 316549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:ignore delay set due to gyro/accel"); 316649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 316749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 316849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 316949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 317049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Orientation: 317149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case RotationVector: 317249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case GameRotationVector: 317349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case GeomagneticRotationVector: 317449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case LinearAccel: 317549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow case Gravity: 317649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isLowPowerQuatEnabled()) { 3177cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 317849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:need to update delay due to LPQ"); 317949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 318049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 318149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 318249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = 0; i < NumSensors; i++) { 318349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { 3184cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 318549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:ignore delay set due to sensor %d", i); 318649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 318749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 318849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 318949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow break; 319049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 319149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 319249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_lock(&mHALMutex); 319349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = update_delay(); 319449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_unlock(&mHALMutex); 319549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 319649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 319749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 319849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::update_delay(void) 319949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 3200cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 320149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 320249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 320349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t got; 320449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 320549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mEnabled) { 320649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t wanted = 1000000000LL; 320749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t wanted_3rd_party_sensor = 1000000000LL; 320849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 320949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Sequence to change sensor's FIFO rate 321049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 1. reset master enable 321149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 2. Update delay 321249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // 3. set master enable 321349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 321449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // reset master enable 321549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(0); 321649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 32174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t gyroRate; 32184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t accelRate; 32194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t compassRate; 32204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t pressureRate; 32214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 32224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int rateInus; 32234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int mplGyroRate; 32244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int mplAccelRate; 32254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int mplCompassRate; 32264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int tempRate = wanted; 32274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 322849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* search the minimum delay requested across all enabled sensors */ 322949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = 0; i < NumSensors; i++) { 323049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mEnabled & (1 << i)) { 323149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t ns = mDelays[i]; 323249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = wanted < ns ? wanted : ns; 323349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 323449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 32354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 32364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mDmpOn) { 32374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow gyroRate = mDelays[Gyro] < mDelays[RawGyro] ? mDelays[Gyro] : mDelays[RawGyro]; 32384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow accelRate = mDelays[Accelerometer]; 32394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = mDelays[MagneticField] < mDelays[RawMagneticField] ? mDelays[MagneticField] : mDelays[RawMagneticField]; 32404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow pressureRate = mDelays[Pressure]; 32414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 32424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#ifdef ENABLE_MULTI_RATE 32434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow gyroRate = wanted; 32444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow accelRate = wanted; 32454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = wanted; 32464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow pressureRate = wanted; 32474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // same delay for 3rd party Accel or Compass 32484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted_3rd_party_sensor = wanted; 32494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#endif 32504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 32514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 32524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow else { 32534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow gyroRate = wanted; 32544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow accelRate = wanted; 32554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = wanted; 32564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow pressureRate = wanted; 32574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // same delay for 3rd party Accel or Compass 32584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted_3rd_party_sensor = wanted; 3259cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 326049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 326149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int enabled_sensors = mEnabled; 326249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int tempFd = -1; 32634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 32644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(mFeatureActiveMask & INV_DMP_BATCH_MODE) { 32654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // set batch rates 32664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask); 32674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV("HAL: batch mode is set, set batch data rates"); 32684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (setBatchDataRates() < 0) { 32694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set batch data rates"); 32704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 32714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 3272cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* set master sampling frequency */ 3273cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int64_t tempWanted = wanted; 3274cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getDmpRate(&tempWanted); 3275cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* driver only looks at sampling frequency if DMP is off */ 3276cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 3277cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp()); 3278cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempFd = open(mpu.gyro_fifo_rate, O_RDWR); 3279cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted); 3280cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE_IF(res < 0, "HAL:sampling frequency update delay error"); 3281cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 328249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (LA_ENABLED || GR_ENABLED || RV_ENABLED 328349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow || GRV_ENABLED || O_ENABLED || GMRV_ENABLED) { 3284cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rateInus = (int)wanted / 1000LL; 3285cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 3286cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* set rate in MPL */ 3287cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* compass can only do 100Hz max */ 3288cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_gyro_sample_rate(rateInus); 3289cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_accel_sample_rate(rateInus); 3290cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_compass_sample_rate(rateInus); 3291cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 3292cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 3293cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", 3294cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rateInus, 1000000000.f / gyroRate); 3295cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 3296cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", 3297cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rateInus, 1000000000.f / accelRate); 3298cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 3299cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", 3300cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rateInus, 1000000000.f / compassRate); 3301cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 3302cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 3303cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "mFeatureActiveMask=%016llx", mFeatureActiveMask); 330449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //TODO: may be able to combine DMP_FEATURE_MASK, DMP_SENSOR_MASK in the future 330549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mFeatureActiveMask & DMP_FEATURE_MASK) { 330649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow bool setDMPrate= 0; 33074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow gyroRate = wanted; 33084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow accelRate = wanted; 33094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = wanted; 33104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // same delay for 3rd party Accel or Compass 33114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted_3rd_party_sensor = wanted; 33124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rateInus = (int)wanted / 1000LL; 33134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 33144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* set rate in MPL */ 33154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* compass can only do 100Hz max */ 3316cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /*inv_set_gyro_sample_rate(rateInus); 33174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_accel_sample_rate(rateInus); 33184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_compass_sample_rate(rateInus); 33194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 33204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 33214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / gyroRate); 33224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 33234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / accelRate); 33244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 33254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", rateInus, 1000000000.f / compassRate); 3326cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro */ 332749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Set LP Quaternion sample rate if enabled 332849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (checkLPQuaternion()) { 332949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (wanted <= RATE_200HZ) { 33304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#ifndef USE_LPQ_AT_FASTEST 333149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(0); 333249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 333349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 333449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_quat_sample_rate(rateInus); 3335cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:MPL quat sample rate: " 3336cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "(mpl)=%d us (mpu)=%.2f Hz", 3337cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rateInus, 1000000000.f / wanted); 333849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow setDMPrate= 1; 333949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 334049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 334149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 334249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 334349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); 334449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //nsToHz 334549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 33464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / gyroRate, mpu.gyro_rate, 334749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getTimestamp()); 33484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.gyro_rate, O_RDWR); 33494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate); 335049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 335149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:GYRO update delay error"); 335249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 335349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 335449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(USE_THIRD_PARTY_ACCEL == 1) { 3355cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // 3rd party accelerometer - if applicable 3356cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // nsToHz (BMA250) 335749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", 33584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted_3rd_party_sensor / 1000000L, mpu.accel_rate, 335949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getTimestamp()); 33604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.accel_rate, O_RDWR); 336149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_attribute_sensor(tempFd, 336249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted_3rd_party_sensor / 1000000L); 336349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 33644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 3365cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // mpu accel 33664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 33674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / accelRate, mpu.accel_rate, 33684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 33694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.accel_rate, O_RDWR); 33704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / accelRate); 33714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 337249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 337349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 337449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!mCompassSensor->isIntegrated()) { 3375cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // stand-alone compass - if applicable 3376cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 337749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:Ext compass delay %lld", wanted_3rd_party_sensor); 3378cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz", 337949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1000000000.f / wanted_3rd_party_sensor); 338049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (wanted_3rd_party_sensor < 338149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->getMinDelay() * 1000LL) { 338249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted_3rd_party_sensor = 338349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->getMinDelay() * 1000LL; 338449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 3385cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 338649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:Ext compass delay %lld", wanted_3rd_party_sensor); 3387cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz", 338849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1000000000.f / wanted_3rd_party_sensor); 338949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor); 339049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow got = mCompassSensor->getDelay(ID_M); 339149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_sample_rate(got / 1000); 33924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 3393cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // compass on secondary bus 33944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (compassRate < mCompassSensor->getMinDelay() * 1000LL) { 33954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = mCompassSensor->getMinDelay() * 1000LL; 33964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 33974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCompassSensor->setDelay(ID_M, compassRate); 339849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 339949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 340049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* 340149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //nsTons - nothing to be done 340249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len], 340349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow COMPASS_SENSOR_DELAY); 340449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tempFd = open(compass_sensor_sysfs_path, O_RDWR); 340549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 340649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "setDelay - open path: %s", compass_sensor_sysfs_path); 340749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = 20000000LLU; 340849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_attribute_sensor(tempFd, wanted); 340949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) { 341049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("Compass update delay error"); 341149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 341249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 341349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 341449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 341549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 341649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GY_ENABLED || RGY_ENABLED) { 341749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = (mDelays[Gyro] <= mDelays[RawGyro]? 341849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): 341949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); 342049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 342149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask & DMP_FEATURE_MASK) { 34224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //int64_t tempWanted; 34234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //getDmpRate(&tempWanted); 342449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 342549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 34264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_gyro_sample_rate((int)wanted/1000LL); 3427cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 34284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL)); 342949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 34304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / wanted, mpu.gyro_rate, getTimestamp()); 34314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.gyro_rate, O_RDWR); 343249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / wanted); 343349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE_IF(res < 0, "HAL:GYRO update delay error"); 343449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 343549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 343649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */ 34374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#if (0) 34384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted = mDelays[Accelerometer]; 34394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#else 344049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { 344149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = mDelays[Gyro]; 344249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (RGY_ENABLED && mDelays[RawGyro] 344349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow < mDelays[Accelerometer]) { 344449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = mDelays[RawGyro]; 344549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 344649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = mDelays[Accelerometer]; 344749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 34484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#endif 344949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 345049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask & DMP_FEATURE_MASK) { 34514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //int64_t tempWanted; 34524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //getDmpRate(&tempWanted); 345349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 345449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 34554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_accel_sample_rate((int)wanted/1000LL); 3456cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:MPL accel sample rate: (mpl)=%d us", 3457cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int(wanted/1000LL)); 345849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* TODO: use function pointers to calculate delay value specific 345949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow to vendor */ 346049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 34614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / wanted, mpu.accel_rate, 346249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getTimestamp()); 34634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.accel_rate, O_RDWR); 346449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(USE_THIRD_PARTY_ACCEL == 1) { 346549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //BMA250 in ms 346649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_attribute_sensor(tempFd, wanted / 1000000L); 346749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 346849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 346949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //MPUxxxx in hz 347049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_attribute_sensor(tempFd, 1000000000.f/wanted); 347149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 347249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 347349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 347449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 347549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Invensense compass calibration */ 347649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (M_ENABLED || RM_ENABLED) { 347749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]? 347849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]): 347949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField])); 348049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!mCompassSensor->isIntegrated()) { 348149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = compassWanted; 348249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 34834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#if (0) 34844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted = compassWanted; 34854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#else 348649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (GY_ENABLED 348749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (mDelays[Gyro] < compassWanted)) { 348849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = mDelays[Gyro]; 348949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (RGY_ENABLED 349049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && mDelays[RawGyro] < compassWanted) { 349149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = mDelays[RawGyro]; 349249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (A_ENABLED && mDelays[Accelerometer] 349349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow < compassWanted) { 349449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = mDelays[Accelerometer]; 349549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 349649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = compassWanted; 349749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 34984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#endif 349949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 350049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask & DMP_FEATURE_MASK) { 35014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //int64_t tempWanted; 35024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //getDmpRate(&tempWanted); 350349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 350449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 350549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 350649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->setDelay(ID_M, wanted); 350749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow got = mCompassSensor->getDelay(ID_M); 350849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_set_compass_sample_rate(got / 1000); 3509cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:MPL compass sample rate: (mpl)=%d us", 3510cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int(got/1000LL)); 351149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 351249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 351349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (PS_ENABLED) { 3514cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int64_t pressureRate = mDelays[Pressure]; 351549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 351649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask & DMP_FEATURE_MASK) { 35174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //int64_t tempWanted; 35184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //getDmpRate(&tempWanted); 351949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 352049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3521cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPressureSensor->setDelay(ID_PS, pressureRate); 352249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE_IF(res < 0, "HAL:PRESSURE update delay error"); 352349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 352449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 352549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 35264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } //end of non batch mode 35274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 352849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned long sensors = mLocalSensorMask & mMasterSensorMask; 352949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sensors & 353049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (INV_THREE_AXIS_GYRO 353149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | INV_THREE_AXIS_ACCEL 353249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated() 353349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())))) { 35344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "sensors=%lu", sensors); 353549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 353649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) 353749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 353849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { // all sensors idle -> reduce power, unless DMP is needed 353949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 354049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mFeatureActiveMask & DMP_FEATURE_MASK) { 354149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 354249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(res < 0) 354349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 354449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 354549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 354649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 354749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 354849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 354949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 355049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 355149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* For Third Party Accel Input Subsystem Drivers only */ 355249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::readAccelEvents(sensors_event_t* data, int count) 355349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 355449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 355549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 355649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (count < 1) 355749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -EINVAL; 355849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 355949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ssize_t n = mAccelInputReader.fill(accel_fd); 356049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (n < 0) { 356149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:missed accel events, exit"); 356249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return n; 356349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 356449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 356549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int numEventReceived = 0; 356649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow input_event const* event; 356749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int nb, done = 0; 356849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 356949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow while (done == 0 && count && mAccelInputReader.readEvent(&event)) { 357049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int type = event->type; 357149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (type == EV_ABS) { 357249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (event->code == EVENT_TYPE_ACCEL_X) { 357349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingMask |= 1 << Accelerometer; 357449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[0] = event->value; 357549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (event->code == EVENT_TYPE_ACCEL_Y) { 357649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingMask |= 1 << Accelerometer; 357749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[1] = event->value; 357849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (event->code == EVENT_TYPE_ACCEL_Z) { 357949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingMask |= 1 << Accelerometer; 358049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[2] =event-> value; 358149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 358249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (type == EV_SYN) { 358349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow done = 1; 358449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { 358549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_build_accel(mCachedAccelData, 0, getTimestamp()); 358649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 358749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 358849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)", 358949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow type, event->code); 359049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 359149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelInputReader.next(); 359249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 359349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 359449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return numEventReceived; 359549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 359649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 359749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 359849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Should be called after reading at least one of gyro 359949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * compass or accel data. (Also okay for handling all of them). 360049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @returns 0, if successful, error number if not. 360149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 360249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::readEvents(sensors_event_t* data, int count) 360349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 3604cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VHANDLER_LOG; 3605cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 360649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_execute_on_data(); 360749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 360849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int numEventReceived = 0; 360949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 361049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long msg; 361149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow msg = inv_get_message_level_0(1); 361249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (msg) { 361349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (msg & INV_MSG_MOTION_EVENT) { 361449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n"); 361549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 361649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (msg & INV_MSG_NO_MOTION_EVENT) { 361749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n"); 361849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* after the first no motion, the gyro should be 361949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow calibrated well */ 362049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH; 362149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* if gyros are on and we got a no motion, set a flag 362249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow indicating that the cal file can be written. */ 362349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mHaveGoodMpuCal = true; 362449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 362549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(msg & INV_MSG_NEW_AB_EVENT) { 3626cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Accel Bias *****\n"); 362749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getAccelBias(); 362849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelAccuracy = inv_get_accel_accuracy(); 362949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 363049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(msg & INV_MSG_NEW_GB_EVENT) { 3631cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Gyro Bias *****\n"); 363249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getGyroBias(); 3633cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setGyroBias(); 363449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 363549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(msg & INV_MSG_NEW_FGB_EVENT) { 3636cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Gyro Bias *****\n"); 363749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getFactoryGyroBias(); 363849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 3639cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(msg & INV_MSG_NEW_FAB_EVENT) { 3640cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Accel Bias *****\n"); 3641cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getFactoryAccelBias(); 3642cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 364349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(msg & INV_MSG_NEW_CB_EVENT) { 3644cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Compass Bias *****\n"); 364549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getCompassBias(); 364649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassAccuracy = inv_get_mag_accuracy(); 364749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 364849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 364949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3650cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // handle flush complete event 3651cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(mFlushBatchSet && mFlushEnabled != -1) { 3652cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sensors_event_t temp; 3653cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int sendEvent = metaHandler(&temp, META_DATA_FLUSH_COMPLETE); 3654cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(sendEvent == 1 && count > 0) { 3655cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro *data++ = temp; 3656cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro count--; 3657cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro numEventReceived++; 3658cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3659cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3660cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 3661cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // handle partial packet read 3662cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mSkipReadEvents) 3663cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return numEventReceived; 3664cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 366549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = 0; i < NumSensors; i++) { 366649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 0; 3667cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 366849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // handle step detector when ped_q is enabled 3669cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(mPedUpdate) { 3670cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (i == StepDetector) { 3671cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro update = readDmpPedometerEvents(data, count, ID_P, 3672cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro SENSOR_TYPE_STEP_DETECTOR, 1); 3673cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPedUpdate = 0; 3674cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(update == 1 && count > 0) { 3675cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro data->timestamp = mStepSensorTimestamp; 3676cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro count--; 3677cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro numEventReceived++; 3678cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro continue; 3679cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3680cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 3681cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mPedUpdate == DATA_FORMAT_STEP) { 3682cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro continue; 3683cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 368449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 368549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 368649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 368749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // load up virtual sensors 368849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mEnabled & (1 << i)) { 368949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i); 369049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPendingMask |= (1 << i); 369149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 369249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (update && (count > 0)) { 369349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *data++ = mPendingEvents[i]; 369449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count--; 369549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow numEventReceived++; 369649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 369749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 369849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 369949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 370049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return numEventReceived; 370149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 370249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 370349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow// collect data for MPL (but NOT sensor service currently), from driver layer 370449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::buildMpuEvent(void) 370549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 3706cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VHANDLER_LOG; 3707cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 3708cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mSkipReadEvents = 0; 37094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0; 371049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int lp_quaternion_on = 0, sixAxis_quaternion_on = 0, 37114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ped_quaternion_on = 0, ped_standalone_on = 0; 371249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow size_t nbyte; 371349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short data_format = 0; 371449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, nb, mask = 0, 371549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) + 371649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) + 371749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) 371849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && mCompassSensor->isIntegrated())? 1 : 0) + 371949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0); 37204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow //LOGV("mLocalSensorMask=0x%lx", mLocalSensorMask); 372149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char *rdata = mIIOBuffer; 37224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ssize_t rsize = 0; 37234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow size_t readCounter = 0; 37244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow char *rdataP = NULL; 372549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 372649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* 2 Bytes header + 6 Bytes x,y,z data | 8 bytes timestamp */ 372749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow nbyte= (BYTES_PER_SENSOR + 8) * sensors * 1; 372849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 372949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* special case for 6 Axis or LPQ */ 373049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* 2 Bytes header + 4 Bytes x data + 2 Bytes n/a */ 373149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* 4 Bytes y data | 4 Bytes z data */ 373249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* 8 Bytes timestamp */ 373349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isLowPowerQuatEnabled()) { 373449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow lp_quaternion_on = checkLPQuaternion(); 373549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (lp_quaternion_on == 1) { 373649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow nbyte += BYTES_QUAT_DATA; 373749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 373849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 373949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 374049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((sixAxis_quaternion_on = check6AxisQuatEnabled())) { 374149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // sixAxis is mutually exclusive to LPQ 374249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // and it is also never enabled when continuous data is enabled 374349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // mLocalSensorMask does not need to be accounted for here 3744cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // because accel/gyro fifo are always turned off 37454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow nbyte += BYTES_QUAT_DATA; 374649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 374749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 374849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((ped_quaternion_on = checkPedQuatEnabled())) { 374949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow nbyte += BYTES_PER_SENSOR_PACKET; 375049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 375149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 37524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((ped_standalone_on = checkPedStandaloneEnabled())) { 37534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow nbyte += BYTES_PER_SENSOR_PACKET; 37544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 37554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 3756cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (checkBatchEnabled()) { 3757cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro nbyte = 24 - mLeftOverBufferSize; 3758cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3759cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 37604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* check previous copied buffer */ 37614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* append with just read data */ 37624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mLeftOverBufferSize > 0) { 37634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize); 37644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize); 37654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0, 37664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input retrieve rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, " 37674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "%d, %d,%d, %d, %d, %d\n", 37684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdata[0], rdata[1], rdata[2], rdata[3], 37694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdata[4], rdata[5], rdata[6], rdata[7], 37704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdata[8], rdata[9], rdata[10], rdata[11], 37714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdata[12], rdata[13], rdata[14], rdata[15]); 37724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 37734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdataP = rdata + mLeftOverBufferSize; 37744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 3775cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* read expected number of bytes */ 3776cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rsize = read(iio_fd, rdataP, nbyte); 377749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(rsize < 0) { 37784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* IIO buffer might have old data. 3779cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro Need to flush it if no sensor is on, to avoid infinite 37804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow read loop.*/ 37814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:input data file descriptor not available - (%s)", 37824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow strerror(errno)); 37834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (sensors == 0) { 37844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE); 37854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 378649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 378749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 3788cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#if 1 3789cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if((rsize + mLeftOverBufferSize) == 8) { 3790cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* store packet then return */ 3791cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro memcpy(mLeftOverBuffer, rdataP, rsize); 3792cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLeftOverBufferSize += rsize; 3793cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(1, "HAL:input data has partial packet"); 3794cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(1, "HAL:input data mLeftOverBufferSize=%d", mLeftOverBufferSize); 3795cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(1, 3796cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input catch up retrieve rdata=:%d, %d, %d, %d, %d, %d, %d, %d", 3797cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3], 3798cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7]); 3799cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mSkipReadEvents = 1; 3800cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return; 3801cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3802cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#endif 38034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* reset data and count pointer */ 38044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdataP = rdata; 3805cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (checkBatchEnabled()) { 3806cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro readCounter = 24; 3807cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3808cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro else { 3809cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro readCounter = rsize + mLeftOverBufferSize; 3810cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 38114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 381249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef TESTING 38134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA, 38144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input rdataP:r=%ld, n=%d," 381549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d\n", 381649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rsize, nbyte, 38174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdataP[0], rdataP[1], rdataP[2], rdataP[3], 38184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdataP[4], rdataP[5], rdataP[6], rdataP[7], 38194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdataP[8], rdataP[9], rdataP[10], rdataP[11], 38204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow rdataP[12], rdataP[13], rdataP[14], rdataP[15]); 382149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 382249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 38234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA && ENG_VERBOSE, 3824cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input b=%d rdata= %d nbyte= %d rsize= %d readCounter= %d", 3825cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro checkBatchEnabled(), *((short *) rdata), nbyte, (int)rsize, readCounter); 38264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA && ENG_VERBOSE, 38274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, " 38284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "ped_q_on= %d, ped_standalone_on= %d", 3829cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on, 3830cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro ped_standalone_on); 38314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 38324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow while (readCounter > 0) { 3833cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // since copied buffer is already accounted for, reset left over size 3834cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLeftOverBufferSize = 0; 3835cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // clear data format mask for parsing the next set of data 38364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mask = 0; 383749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data_format = *((short *)(rdata)); 38384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA && ENG_VERBOSE, 38394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input data_format=%x", data_format); 384049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 384149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((data_format & ~DATA_FORMAT_MASK) || (data_format == 0)) { 38424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:input invalid data_format 0x%02X", data_format); 384349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 384449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 38454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 38464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (data_format & DATA_FORMAT_STEP) { 3847cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (data_format == DATA_FORMAT_STEP) { 3848cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro rdata += BYTES_PER_SENSOR; 3849cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro latestTimestamp = *((long long*) (rdata)); 3850cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "STEP DETECTED:0x%x - ts: %lld", data_format, latestTimestamp); 3851cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // readCounter is decrement by 24 because DATA_FORMAT_STEP only applies in batch mode 3852cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro readCounter -= BYTES_PER_SENSOR_PACKET; 3853cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 3854cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(0, "STEP DETECTED:0x%x", data_format); 3855cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 3856cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPedUpdate |= data_format; 38574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mask |= DATA_FORMAT_STEP; 38584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // cancels step bit 38594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow data_format &= (~DATA_FORMAT_STEP); 38604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 3861cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 3862cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (data_format & DATA_FORMAT_MARKER) { 3863cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "MARKER DETECTED:0x%x", data_format); 3864cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // cancels marker bit 3865cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro data_format &= (~DATA_FORMAT_MARKER); 3866cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushBatchSet = 1; 3867cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 38684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 386949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data_format == DATA_FORMAT_QUAT) { 387049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedQuaternionData[0] = *((int *) (rdata + 4)); 387149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedQuaternionData[1] = *((int *) (rdata + 8)); 38724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCachedQuaternionData[2] = *((int *) (rdata + 12)); 387349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += QUAT_ONLY_LAST_PACKET_OFFSET; 387449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp = *((long long*) (rdata)); 387549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mask |= DATA_FORMAT_QUAT; 387649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_QUAT_DATA; 387749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 387849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (data_format == DATA_FORMAT_6_AXIS) { 387949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCached6AxisQuaternionData[0] = *((int *) (rdata + 4)); 388049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCached6AxisQuaternionData[1] = *((int *) (rdata + 8)); 388149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCached6AxisQuaternionData[2] = *((int *) (rdata + 12)); 388249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += QUAT_ONLY_LAST_PACKET_OFFSET; 388349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp = *((long long*) (rdata)); 388449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mask |= DATA_FORMAT_6_AXIS; 388549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_QUAT_DATA; 388649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 388749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (data_format == DATA_FORMAT_PED_QUAT) { 388849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedPedQuaternionData[0] = *((short *) (rdata + 2)); 388949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedPedQuaternionData[1] = *((short *) (rdata + 4)); 389049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedPedQuaternionData[2] = *((short *) (rdata + 6)); 389149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 389249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp = *((long long*) (rdata)); 389349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mask |= DATA_FORMAT_PED_QUAT; 389449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_PER_SENSOR_PACKET; 389549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 38964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow else if (data_format == DATA_FORMAT_PED_STANDALONE) { 38974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "STEP DETECTED:0x%x", data_format); 389849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 38994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mStepSensorTimestamp = *((long long*) (rdata)); 39004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mask |= DATA_FORMAT_PED_STANDALONE; 390149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_PER_SENSOR_PACKET; 3902cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPedUpdate |= data_format; 390349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 390449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (data_format == DATA_FORMAT_GYRO) { 390549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedGyroData[0] = *((short *) (rdata + 2)); 390649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedGyroData[1] = *((short *) (rdata + 4)); 390749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedGyroData[2] = *((short *) (rdata + 6)); 390849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 390949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroSensorTimestamp = *((long long*) (rdata)); 391049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mask |= DATA_FORMAT_GYRO; 391149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_PER_SENSOR_PACKET; 391249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 391349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (data_format == DATA_FORMAT_ACCEL) { 391449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[0] = *((short *) (rdata + 2)); 391549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[1] = *((short *) (rdata + 4)); 391649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[2] = *((short *) (rdata + 6)); 391749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 391849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelSensorTimestamp = *((long long*) (rdata)); 391949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mask |= DATA_FORMAT_ACCEL; 392049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_PER_SENSOR_PACKET; 392149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 39224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow else if (data_format == DATA_FORMAT_COMPASS) { 392349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mCompassSensor->isIntegrated()) { 392449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[0] = *((short *) (rdata + 2)); 392549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[1] = *((short *) (rdata + 4)); 392649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[2] = *((short *) (rdata + 6)); 392749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 392849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassTimestamp = *((long long*) (rdata)); 3929cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mask |= DATA_FORMAT_COMPASS; 393049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow readCounter -= BYTES_PER_SENSOR_PACKET; 393149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 393249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 393349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else if (data_format == DATA_FORMAT_PRESSURE) { 393449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mPressureSensor->isIntegrated()) { 39354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCachedPressureData = 39364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ((*((short *)(rdata + 4))) << 16) + 39374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (*((unsigned short *) (rdata + 6))); 393849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 393949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mPressureTimestamp = *((long long*) (rdata)); 3940cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mCachedPressureData != 0) { 394149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mask |= DATA_FORMAT_PRESSURE; 394249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 39434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow readCounter -= BYTES_PER_SENSOR_PACKET; 394449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 394549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 394649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow rdata += BYTES_PER_SENSOR; 394749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3948cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro size_t storeBufferSize = 0; 3949cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (checkBatchEnabled()) { 3950cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro storeBufferSize = 8; 3951cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 3952cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro storeBufferSize = 24; 3953cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 39544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* read ahead and store left over data if any */ 3955cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((readCounter != 0) && ((checkBatchEnabled() && (rsize == (ssize_t)nbyte)) || 3956cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (!checkBatchEnabled() && (rsize != (ssize_t)nbyte))) 3957cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro &&(readCounter <= storeBufferSize)) { 39584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int currentBufferCounter = 0; 3959cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(0, "!!! not enough data readCounter=%d, expected nbyte=%d, rsize=%d", readCounter, nbyte, (int)rsize); 39604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow memcpy(mLeftOverBuffer, rdata, readCounter); 39614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0, 39624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, " 39634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "%d, %d, %d,%d, %d, %d, %d\n", 39644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3], 39654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7], 39664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11], 39674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]); 39684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 39694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mLeftOverBufferSize = readCounter; 39704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow readCounter = 0; 39714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0, "!!! stored number of bytes:%d", mLeftOverBufferSize); 3972cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (checkBatchEnabled()) { 3973cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* reset count since this is the last packet for the data set */ 3974cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro readCounter = 0; 3975cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mLeftOverBufferSize = 0; 39764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 39774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 39784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* handle data read */ 39794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mask & DATA_FORMAT_GYRO) { 39804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* batch mode does not batch temperature */ 39814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* disable temperature read */ 39824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) { 39834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // send down temperature every 0.5 seconds 39844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // with timestamp measured in "driver" layer 39854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) { 39864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mTempCurrentTime = mGyroSensorTimestamp; 39874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow long long temperature[2]; 39884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(inv_read_temperature(temperature) == 0) { 39894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA, 39904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input inv_read_temperature = %lld, timestamp= %lld", 399149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temperature[0], temperature[1]); 39924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_build_temp(temperature[0], temperature[1]); 39934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 399449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef TESTING 39954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow long bias[3], temp, temp_slope[3]; 39964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_get_mpl_gyro_bias(bias, &temp); 39974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_get_gyro_ts(temp_slope); 39984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGI("T: %.3f " 39994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "GB: %+13f %+13f %+13f " 40004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "TS: %+13f %+13f %+13f " 40014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "\n", 40024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (float)temperature[0] / 65536.f, 40034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (float)bias[0] / 65536.f / 16.384f, 40044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (float)bias[1] / 65536.f / 16.384f, 40054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (float)bias[2] / 65536.f / 16.384f, 40064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow temp_slope[0] / 65536.f, 40074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow temp_slope[1] / 65536.f, 40084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow temp_slope[2] / 65536.f); 400949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 40104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mPendingMask |= 1 << Gyro; 40134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mPendingMask |= 1 << RawGyro; 40144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 40154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { 40164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp); 40174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA, 40184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld", 40194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCachedGyroData[0], mCachedGyroData[1], 40204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCachedGyroData[2], mGyroSensorTimestamp); 40214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow latestTimestamp = mGyroSensorTimestamp; 402349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 402449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mask & DATA_FORMAT_ACCEL) { 40264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mPendingMask |= 1 << Accelerometer; 40274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { 40284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp); 40294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA, 40304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld", 403149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[0], mCachedAccelData[1], 403249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedAccelData[2], mAccelSensorTimestamp); 4033cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* remember inital 6 axis quaternion */ 4034cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_time_t tempTimestamp; 4035cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp); 4036cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mInitial6QuatValue[0] != 0 || mInitial6QuatValue[1] != 0 || 4037cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mInitial6QuatValue[2] != 0 || mInitial6QuatValue[3] != 0) { 4038cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mInitial6QuatValueAvailable = 1; 4039cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(INPUT_DATA && ENG_VERBOSE, 4040cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld", 4041cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mInitial6QuatValue[0], mInitial6QuatValue[1], 4042cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mInitial6QuatValue[2], mInitial6QuatValue[3]); 4043cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 4044cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 40454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow latestTimestamp = mAccelSensorTimestamp; 404749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 404849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mask & DATA_FORMAT_COMPASS) && mCompassSensor->isIntegrated()) { 40504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int status = 0; 40514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mCompassSensor->providesCalibration()) { 40524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status = mCompassSensor->getAccuracy(); 40534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= INV_CALIBRATED; 40544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { 405649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_build_compass(mCachedCompassData, status, 405749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassTimestamp); 405849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(INPUT_DATA, 40594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld", 406049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[0], mCachedCompassData[1], 406149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[2], mCompassTimestamp); 40624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow latestTimestamp = mCompassTimestamp; 406449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 406549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (isLowPowerQuatEnabled() && lp_quaternion_on == 1 406749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (mask & DATA_FORMAT_QUAT)) { 40684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* if bias was applied to DMP bias, 40694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow set status bits to disable gyro bias cal */ 40704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int status = 0; 40714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mGyroBiasApplied == true) { 4072cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used"); 40734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= INV_BIAS_APPLIED; 40744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */ 40764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_build_quat(mCachedQuaternionData, 407749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status, 407849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp); 40794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA, 4080cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input inv_build_quat-3x: %+8ld %+8ld %+8ld - %lld", 40814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCachedQuaternionData[0], mCachedQuaternionData[1], 40824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCachedQuaternionData[2], 40834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mQuatSensorTimestamp); 40844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow latestTimestamp = mQuatSensorTimestamp; 40854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 408649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 40874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mask & DATA_FORMAT_6_AXIS) && check6AxisQuatEnabled() 408849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow && (sixAxis_quaternion_on == 1)) { 40894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* if bias was applied to DMP bias, 40904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow set status bits to disable gyro bias cal */ 40914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int status = 0; 40924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mGyroBiasApplied == true) { 4093cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used"); 40944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= INV_QUAT_6AXIS; 40954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 40964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */ 40974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_build_quat(mCached6AxisQuaternionData, 409849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status, 409949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp); 41004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(INPUT_DATA, 4101cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld", 4102cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1], 4103cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCached6AxisQuaternionData[2], mQuatSensorTimestamp); 41044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow latestTimestamp = mQuatSensorTimestamp; 410549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 41074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mask & DATA_FORMAT_PED_QUAT) && checkPedQuatEnabled() 41084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow && (ped_quaternion_on == 1)) { 41094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* if bias was applied to DMP bias, 41104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow set status bits to disable gyro bias cal */ 41114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int status = 0; 41124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mGyroBiasApplied == true) { 4113cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(INPUT_DATA && ENG_VERBOSE, 4114cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input dmp bias is used"); 41154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= INV_QUAT_6AXIS; 41164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 41174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow status |= 32 | INV_QUAT_3AXIS; /* default 32 (16/32bits) */ 4118cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCachedPedQuaternionData[0] = mCachedPedQuaternionData[0] << 16; 4119cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCachedPedQuaternionData[1] = mCachedPedQuaternionData[1] << 16; 4120cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCachedPedQuaternionData[2] = mCachedPedQuaternionData[2] << 16; 41214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_build_quat(mCachedPedQuaternionData, 412249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status, 412349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mQuatSensorTimestamp); 41244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 412549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(INPUT_DATA, 4126cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld", 4127cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCachedPedQuaternionData[0], mCachedPedQuaternionData[1], 4128cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCachedPedQuaternionData[2], mQuatSensorTimestamp); 4129cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro latestTimestamp = mQuatSensorTimestamp; 413049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 413149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 41324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mask & DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) { 41334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int status = 0; 41344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) { 4135cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro latestTimestamp = mPressureTimestamp; 4136cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPressureUpdate = 1; 4137cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_build_pressure(mCachedPressureData, 4138cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro status, 4139cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPressureTimestamp); 4140cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(INPUT_DATA, 41414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input inv_build_pressure: %+8ld - %lld", 4142cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mCachedPressureData, mPressureTimestamp); 41434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 414449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 41454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 41464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* take the latest timestamp */ 41474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mask & DATA_FORMAT_STEP) { 4148cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* work around driver output duplicate step detector bit */ 4149cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (latestTimestamp > mStepSensorTimestamp) { 4150cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mStepSensorTimestamp = latestTimestamp; 4151cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(INPUT_DATA, 4152cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:input build step: 1 - %lld", mStepSensorTimestamp); 4153cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 4154cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPedUpdate = 0; 4155cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 41564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 4157cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } //while end 415849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 415949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 416049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* use for both MPUxxxx and third party compass */ 416149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::buildCompassEvent(void) 416249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 416349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 416449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 416549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int done = 0; 416649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 416749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_lock(&mMplMutex); 416849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_lock(&mHALMutex); 416949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 417049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); 417149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mCompassSensor->isYasCompass()) { 417249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mCompassSensor->checkCoilsReset() == 1) { 417349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Reset relevant compass settings 417449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow resetCompass(); 417549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 417649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 417749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (done > 0) { 417849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int status = 0; 417949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mCompassSensor->providesCalibration()) { 418049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status = mCompassSensor->getAccuracy(); 418149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow status |= INV_CALIBRATED; 418249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 418349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { 418449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_build_compass(mCachedCompassData, status, 418549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassTimestamp); 418649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(INPUT_DATA, 41874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld", 418849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[0], mCachedCompassData[1], 418949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCachedCompassData[2], mCompassTimestamp); 419049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 419149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 419249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 419349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_unlock(&mMplMutex); 419449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // pthread_mutex_unlock(&mHALMutex); 419549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 419649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 419749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::resetCompass(void) 419849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 419949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 420049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 420149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Reset compass cal if enabled 420249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mMplFeatureActiveMask & INV_COMPASS_CAL) { 420349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal"); 420449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_init_vector_compass_cal(); 420549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 420649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 420749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Reset compass fit if enabled 420849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mMplFeatureActiveMask & INV_COMPASS_FIT) { 420949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit"); 421049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_init_compass_fit(); 421149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 421249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 421349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 421449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 421549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 421649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getFd(void) const 421749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 421849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 4219cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "getFd returning %d", iio_fd); 422049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return iio_fd; 422149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 422249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 422349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getAccelFd(void) const 422449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 422549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 4226cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "getAccelFd returning %d", accel_fd); 422749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return accel_fd; 422849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 422949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 423049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getCompassFd(void) const 423149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 423249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 423349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int fd = mCompassSensor->getFd(); 4234cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "getCompassFd returning %d", fd); 423549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return fd; 423649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 423749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 423849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::turnOffAccelFifo(void) 423949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4240cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 424149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, res = 0, tempFd; 424249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 424349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.accel_fifo_enable, getTimestamp()); 424449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res += write_sysfs_int(mpu.accel_fifo_enable, 0); 424549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 424649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 424749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 424849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::turnOffGyroFifo(void) 424949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4250cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 425149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int i, res = 0, tempFd; 425249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 425349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.gyro_fifo_enable, getTimestamp()); 425449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res += write_sysfs_int(mpu.gyro_fifo_enable, 0); 425549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 425649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 425749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 425849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableDmpOrientation(int en) 425949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 426049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 426149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 426249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int enabled_sensors = mEnabled; 426349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 426449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isMpuNonDmp()) 426549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 426649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 426749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // reset master enable 426849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 426949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 427049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 427149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 427249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (en == 1) { 427349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Enable DMP orientation 427449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 427549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.display_orientation_on, getTimestamp()); 427649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { 427749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable Android orientation"); 427849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; // indicate an err 427949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 428049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 428149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 428249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable DMP 428349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(1); 428449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 428549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 428649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 42874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // set rate to 200Hz 428849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 428949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 200, mpu.accel_fifo_rate, getTimestamp()); 429049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { 429149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 42924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set rate to 200Hz"); 429349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 429449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 429549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 429649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable accel engine 429749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(1); 429849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 429949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 430049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 430149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel FIFO 430249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { 430349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = turnOffAccelFifo(); 430449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 430549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 430649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 430749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 430849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!mEnabled){ 430949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 431049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1, mpu.dmp_event_int_on, getTimestamp()); 431149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 431249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 431349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 431449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 431549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 431649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 431749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION; 431849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 431949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 432049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION; 432149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP 432249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask == 0) { 432349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(0); 432449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 432549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 432649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 432749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel engine 432849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask 432949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_ACCEL)) { 433049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(0); 433149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 433249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 433349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 433449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 433549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 433649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mEnabled){ 433749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 433849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_event_int_on, getTimestamp()); 433949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 434049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 434149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 434249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 434349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 434449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask); 434549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 434649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 434749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (en || mEnabled || mFeatureActiveMask) { 434849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 434949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 435049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 435149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 435249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 435349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::openDmpOrientFd(void) 435449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 435549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 435649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 435749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) { 435849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 435949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:DMP display orientation disabled or file desc opened"); 436049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 436149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 436249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 436349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK); 436449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (dmp_orient_fd < 0) { 436549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR couldn't open dmpOrient node"); 436649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 436749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 436849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 436949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:dmp_orient_fd opened : %d", dmp_orient_fd); 437049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 437149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 437249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 437349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 437449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::closeDmpOrientFd(void) 437549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 437649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 437749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (dmp_orient_fd >= 0) 437849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(dmp_orient_fd); 437949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 438049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 438149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 438249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::dmpOrientHandler(int orient) 438349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 438449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 438549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient); 438649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 438749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 438849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 438949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) 439049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 439149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 439249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 439349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char dummy[4]; 439449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int screen_orientation = 0; 439549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *fp; 439649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 439749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fp = fopen(mpu.event_display_orientation, "r"); 439849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (fp == NULL) { 439949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:cannot open event_display_orientation"); 440049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 4401cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 4402cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fscanf(fp, "%d\n", &screen_orientation) < 0 || fclose(fp) < 0) 4403cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro { 4404cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:cannot write event_display_orientation"); 4405cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 440649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 440749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 440849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int numEventReceived = 0; 440949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 441049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mDmpOrientationEnabled && count > 0) { 441149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors_event_t temp; 441249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4413cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro temp.acceleration.x = 0; 4414cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro temp.acceleration.y = 0; 4415cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro temp.acceleration.z = 0; 441649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.version = sizeof(sensors_event_t); 441749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.sensor = ID_SO; 441849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.acceleration.status 441949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_UNRELIABLE; 442049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 442149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.type = SENSOR_TYPE_SCREEN_ORIENTATION; 442249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.screen_orientation = screen_orientation; 442349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 442449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct timespec ts; 442549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow clock_gettime(CLOCK_MONOTONIC, &ts); 442649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 442749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 442849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *data++ = temp; 442949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count--; 443049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow numEventReceived++; 443149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 443249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 443349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // read dummy data per driver's request 443449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmpOrientHandler(screen_orientation); 443549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow read(dmp_orient_fd, dummy, 4); 443649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 443749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return numEventReceived; 443849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 443949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 444049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getDmpOrientFd(void) 444149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 444249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 444349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4444cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "getDmpOrientFd returning %d", dmp_orient_fd); 444549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return dmp_orient_fd; 444649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 444749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 444849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 444949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::checkDMPOrientation(void) 445049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 445149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 445249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0); 445349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 445449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 445549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getDmpRate(int64_t *wanted) 445649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 445749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 445849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 445949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set DMP output rate to FIFO 446049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mDmpOn == 1) { 446149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 44624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int(1000000000.f / *wanted), mpu.three_axis_q_rate, 446349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getTimestamp()); 44644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / *wanted); 446549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 4466cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:DMP three axis rate %.2f Hz", 1000000000.f / *wanted); 4467cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mFeatureActiveMask & INV_DMP_BATCH_MODE) { 4468cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 4469cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int(1000000000.f / *wanted), mpu.six_axis_q_rate, 4470cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getTimestamp()); 4471cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / *wanted); 4472cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, 4473cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:DMP six axis rate %.2f Hz", 1000000000.f / *wanted); 4474cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 4475cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 4476cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int(1000000000.f / *wanted), mpu.ped_q_rate, 4477cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getTimestamp()); 4478cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro write_sysfs_int(mpu.ped_q_rate, 1000000000.f / *wanted); 4479cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, 4480cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / *wanted); 4481cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 448249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //DMP running rate must be @ 200Hz 448349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *wanted= RATE_200HZ; 448449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(PROCESS_VERBOSE, 448549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted); 448649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 448749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 448849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 448949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 449049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getPollTime(void) 449149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4492cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 449349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return mPollTime; 449449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 449549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 449649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getStepCountPollTime(void) 449749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4498cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 449949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mDmpStepCountEnabled) { 4500cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* clamped to 1ms?, still rather large */ 4501cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(0/*EXTRA_VERBOSE*/, "Step Count poll time = %lld ms", 4502cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mStepCountPollTime / 1000000LL); 450349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (mStepCountPollTime / 1000000LL); 450449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 4505cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 1000; 450649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 450749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 450849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowbool MPLSensor::hasStepCountPendingEvents(void) 450949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4510cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 451149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mDmpStepCountEnabled) { 451249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct timespec t_now; 451349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t interval = 0; 451449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 451549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow clock_gettime(CLOCK_MONOTONIC, &t_now); 451649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) - 451749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec)); 451849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 451949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (interval < mStepCountPollTime) { 45204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0/*ENG_VERBOSE*/, 45214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "Step Count interval elapsed: %lld, triggered: %d", 452249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow interval, mStepCountPollTime); 452349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return false; 452449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 452549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow clock_gettime(CLOCK_MONOTONIC, &mt_pre); 45264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(0/*ENG_VERBOSE*/, "Step Count previous time: %ld ms", 452749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mt_pre.tv_nsec / 1000); 452849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return true; 452949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 453049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 453149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return false; 453249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 453349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 453449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowbool MPLSensor::hasPendingEvents(void) const 453549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4536cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 453749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // if we are using the polling workaround, force the main 453849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // loop to check for data every time 453949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (mPollTime != -1); 454049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 454149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 454249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* TODO: support resume suspend when we gain more info about them*/ 454349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::sleepEvent(void) 454449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 454549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 454649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 454749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 454849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::wakeEvent(void) 454949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 455049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 455149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 455249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 455349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_float_to_q16(float *fdata, long *ldata) 455449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4555cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //VFUNC_LOG; 455649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 455749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!fdata || !ldata) 455849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 455949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[0] = (long)(fdata[0] * 65536.f); 456049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[1] = (long)(fdata[1] * 65536.f); 456149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[2] = (long)(fdata[2] * 65536.f); 456249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 456349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 456449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 456549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_long_to_q16(long *fdata, long *ldata) 456649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4567cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //VFUNC_LOG; 456849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 456949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!fdata || !ldata) 457049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 457149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[0] = (fdata[1] * 65536.f); 457249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[1] = (fdata[2] * 65536.f); 457349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[2] = (fdata[3] * 65536.f); 457449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 457549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 457649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 457749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_float_to_round(float *fdata, long *ldata) 457849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 457949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 458049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 458149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!fdata || !ldata) 458249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 458349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[0] = (long)fdata[0]; 458449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[1] = (long)fdata[1]; 458549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[2] = (long)fdata[2]; 458649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 458749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 458849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 458949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_float_to_round2(float *fdata, short *ldata) 459049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4591cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //VFUNC_LOG; 459249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 459349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!fdata || !ldata) 459449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 459549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[0] = (short)fdata[0]; 459649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[1] = (short)fdata[1]; 459749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ldata[2] = (short)fdata[2]; 459849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 459949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 460049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 460149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_long_to_float(long *ldata, float *fdata) 460249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 4603cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro //VFUNC_LOG; 460449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 460549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!ldata || !fdata) 460649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 460749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fdata[0] = (float)ldata[0]; 460849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fdata[1] = (float)ldata[1]; 460949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fdata[2] = (float)ldata[2]; 461049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 461149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 461249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 461349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_read_temperature(long long *data) 461449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 461549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VHANDLER_LOG; 461649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 461749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int count = 0; 461849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char raw_buf[40]; 461949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long raw = 0; 462049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 462149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long long timestamp = 0; 462249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 462349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(raw_buf, 0, sizeof(raw_buf)); 462449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = read_attribute_sensor(gyro_temperature_fd, raw_buf, 462549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sizeof(raw_buf)); 462649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 1) { 462749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:error reading gyro temperature"); 462849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 462949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 463049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 463149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp); 463249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 463349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 0) { 463449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 463549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 463649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 463749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, 463849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:temperature raw = %ld, timestamp = %lld, count = %d", 463949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow raw, timestamp, count); 464049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] = raw; 464149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[1] = timestamp; 464249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 464349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 464449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 464549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 464649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_read_dmp_state(int fd) 464749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 464849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 464949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 465049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fd < 0) 465149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 465249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 465349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int count = 0; 465449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char raw_buf[10]; 465549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow short raw = 0; 465649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 465749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(raw_buf, 0, sizeof(raw_buf)); 465849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); 465949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 1) { 466049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:error reading dmp state"); 466149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(fd); 466249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 466349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 466449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = sscanf(raw_buf, "%hd", &raw); 466549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 0) { 466649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:dmp state data is invalid"); 466749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(fd); 466849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 466949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 467049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count); 467149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow close(fd); 467249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return (int)raw; 467349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 467449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 467549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_read_sensor_bias(int fd, long *data) 467649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 467749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 467849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 467949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(fd == -1) { 468049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 468149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 468249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 468349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char buf[50]; 468449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char x[15], y[15], z[15]; 468549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 468649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(buf, 0, sizeof(buf)); 468749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int count = read_attribute_sensor(fd, buf, sizeof(buf)); 468849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count < 1) { 468949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error reading gyro bias"); 469049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 469149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 469249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z); 469349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(count) { 469449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* scale appropriately for MPL */ 469549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, 469649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)", 469749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow atol(x), atol(y), atol(z)); 469849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 469949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0] = (long)(atol(x) / 10000 * (1L << 16)); 470049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[1] = (long)(atol(y) / 10000 * (1L << 16)); 470149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[2] = (long)(atol(z) / 10000 * (1L << 16)); 470249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 470349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(ENG_VERBOSE, 470449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)", 470549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data[0], data[1], data[2]); 470649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 470749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 470849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 470949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 471049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** fill in the sensor list based on which sensors are configured. 471149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * return the number of configured sensors. 471249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * parameter list must point to a memory region of at least 7*sizeof(sensor_t) 471349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * parameter len gives the length of the buffer pointed to by list 471449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 471549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::populateSensorList(struct sensor_t *list, int len) 471649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 471749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 471849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 471949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int numsensors; 472049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 472149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(len < 472249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { 472349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:sensor list too small, not populating."); 472449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -(sizeof(sSensorList) / sizeof(sensor_t)); 472549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 472649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 472749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in the base values */ 472849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(list, sSensorList, 472949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t))); 473049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 473149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* first add gyro, accel and compass to the list */ 473249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 473349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in gyro/accel values */ 473449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(chip_ID == NULL) { 473549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Can not get gyro/accel id"); 473649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 473749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillGyro(chip_ID, list); 473849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillAccel(chip_ID, list); 473949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 474049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // TODO: need fixes for unified HAL and 3rd-party solution 474149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->fillList(&list[MagneticField]); 474249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->fillList(&list[RawMagneticField]); 474349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4744cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mPressureSensor != NULL) { 4745cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mPressureSensor->fillList(&list[Pressure]); 4746cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 4747cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 474849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(1) { 474949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow numsensors = (sizeof(sSensorList) / sizeof(sensor_t)); 475049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* all sensors will be added to the list 475149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fill in orientation values */ 475249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillOrientation(list); 475349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in rotation vector values */ 475449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillRV(list); 475549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in game rotation vector values */ 475649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillGRV(list); 475749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in gravity values */ 475849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillGravity(list); 475949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in Linear accel values */ 476049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillLinearAccel(list); 476149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in Significant motion values */ 476249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillSignificantMotion(list); 476349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 476449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* fill in screen orientation values */ 476549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fillScreenOrientation(list); 476649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 476749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 476849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* no 9-axis sensors, zero fill that part of the list */ 476949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow numsensors = 3; 477049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(list + 3, 0, 4 * sizeof(struct sensor_t)); 477149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 477249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 477349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return numsensors; 477449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 477549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 477649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillAccel(const char* accel, struct sensor_t *list) 477749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 477849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 477949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 478049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (accel) { 478149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(accel != NULL && strcmp(accel, "BMA250") == 0) { 478249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 478349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 478449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_BMA250_POWER; 478549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; 478649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 478749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) { 478849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; 478949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; 479049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_MPU6050_POWER; 479149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY; 479249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 479349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) { 479449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; 479549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; 479649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_MPU6500_POWER; 479749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; 479849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 479949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (accel != NULL && strcmp(accel, "MPU6515") == 0) { 480049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; 480149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; 480249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_MPU6500_POWER; 480349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; 480449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 480549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) { 480649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; 480749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; 480849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_MPU6500_POWER; 480949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; 481049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 481149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) { 481249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; 481349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; 481449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_MPU6500_POWER; 481549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; 481649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 481749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) { 481849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE; 481949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION; 482049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_MPU9150_POWER; 482149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY; 482249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 4823cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (accel != NULL && strcmp(accel, "MPU9250") == 0) { 4824cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].maxRange = ACCEL_MPU9250_RANGE; 4825cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].resolution = ACCEL_MPU9250_RESOLUTION; 4826cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].power = ACCEL_MPU9250_POWER; 4827cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].minDelay = ACCEL_MPU9250_MINDELAY; 4828cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return; 4829cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (accel != NULL && strcmp(accel, "MPU9350") == 0) { 4830cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].maxRange = ACCEL_MPU9350_RANGE; 4831cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].resolution = ACCEL_MPU9350_RESOLUTION; 4832cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].power = ACCEL_MPU9350_POWER; 4833cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Accelerometer].minDelay = ACCEL_MPU9350_MINDELAY; 4834cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return; 4835cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { 483649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 483749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 483849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_BMA250_POWER; 483949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; 484049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 484149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 484249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 484349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 484449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:unknown accel id %s -- " 484549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "params default to bma250 and might be wrong.", 484649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow accel); 484749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 484849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 484949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power = ACCEL_BMA250_POWER; 485049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; 485149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 485249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 485349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 485449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 485549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 485649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 485749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) { 485849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].maxRange = GYRO_MPU3050_RANGE; 485949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 486049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].power = GYRO_MPU3050_POWER; 486149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; 486249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) { 486349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].maxRange = GYRO_MPU6050_RANGE; 486449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; 486549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].power = GYRO_MPU6050_POWER; 486649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].minDelay = GYRO_MPU6050_MINDELAY; 486749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) { 486849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].maxRange = GYRO_MPU6500_RANGE; 486949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].resolution = GYRO_MPU6500_RESOLUTION; 487049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].power = GYRO_MPU6500_POWER; 487149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].minDelay = GYRO_MPU6500_MINDELAY; 487249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if( gyro != NULL && strcmp(gyro, "MPU6515") == 0) { 487349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].maxRange = GYRO_MPU6500_RANGE; 487449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].resolution = GYRO_MPU6500_RESOLUTION; 487549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].power = GYRO_MPU6500_POWER; 487649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].minDelay = GYRO_MPU6500_MINDELAY; 487749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) { 487849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].maxRange = GYRO_MPU9150_RANGE; 487949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].resolution = GYRO_MPU9150_RESOLUTION; 488049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].power = GYRO_MPU9150_POWER; 488149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].minDelay = GYRO_MPU9150_MINDELAY; 4882cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if( gyro != NULL && strcmp(gyro, "MPU9250") == 0) { 4883cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].maxRange = GYRO_MPU9250_RANGE; 4884cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].resolution = GYRO_MPU9250_RESOLUTION; 4885cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].power = GYRO_MPU9250_POWER; 4886cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].minDelay = GYRO_MPU9250_MINDELAY; 4887cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if( gyro != NULL && strcmp(gyro, "MPU9350") == 0) { 4888cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].maxRange = GYRO_MPU9350_RANGE; 4889cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].resolution = GYRO_MPU9350_RESOLUTION; 4890cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].power = GYRO_MPU9350_POWER; 4891cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro list[Gyro].minDelay = GYRO_MPU9350_MINDELAY; 489249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 489349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:unknown gyro id -- gyro params will be wrong."); 489449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:default to use mpu3050 params"); 489549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].maxRange = GYRO_MPU3050_RANGE; 489649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 489749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].power = GYRO_MPU3050_POWER; 489849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; 489949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 490049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 490149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RawGyro].maxRange = list[Gyro].maxRange; 490249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RawGyro].resolution = list[Gyro].resolution; 490349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RawGyro].power = list[Gyro].power; 490449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RawGyro].minDelay = list[Gyro].minDelay; 490549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 490649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 490749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 490849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 490949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* fillRV depends on values of gyro, accel and compass in the list */ 491049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillRV(struct sensor_t *list) 491149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 491249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 491349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 491449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* compute power on the fly */ 491549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RotationVector].power = list[Gyro].power + 491649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power + 491749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[MagneticField].power; 491849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RotationVector].resolution = .00001; 491949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RotationVector].maxRange = 1.0; 492049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[RotationVector].minDelay = 5000; 492149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 492249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 492349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 492449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 492549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* fillGMRV depends on values of accel and mag in the list */ 492649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillGMRV(struct sensor_t *list) 492749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 492849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 492949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 493049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* compute power on the fly */ 493149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GeomagneticRotationVector].power = list[Accelerometer].power + 493249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[MagneticField].power; 493349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GeomagneticRotationVector].resolution = .00001; 493449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GeomagneticRotationVector].maxRange = 1.0; 493549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GeomagneticRotationVector].minDelay = 5000; 493649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 493749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 493849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 493949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 494049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* fillGRV depends on values of gyro and accel in the list */ 494149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillGRV(struct sensor_t *list) 494249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 494349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 494449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 494549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* compute power on the fly */ 494649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GameRotationVector].power = list[Gyro].power + 494749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power; 494849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GameRotationVector].resolution = .00001; 494949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GameRotationVector].maxRange = 1.0; 495049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[GameRotationVector].minDelay = 5000; 495149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 495249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 495349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 495449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 495549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillOrientation(struct sensor_t *list) 495649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 495749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 495849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 495949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Orientation].power = list[Gyro].power + 496049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power + 496149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[MagneticField].power; 496249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Orientation].resolution = .00001; 496349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Orientation].maxRange = 360.0; 496449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Orientation].minDelay = 5000; 496549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 496649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 496749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 496849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 496949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillGravity( struct sensor_t *list) 497049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 497149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 497249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 497349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gravity].power = list[Gyro].power + 497449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power + 497549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[MagneticField].power; 497649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gravity].resolution = .00001; 497749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gravity].maxRange = 9.81; 497849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Gravity].minDelay = 5000; 497949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 498049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 498149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 498249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 498349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillLinearAccel(struct sensor_t *list) 498449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 498549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 498649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 498749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[LinearAccel].power = list[Gyro].power + 498849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[Accelerometer].power + 498949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[MagneticField].power; 499049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[LinearAccel].resolution = list[Accelerometer].resolution; 499149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[LinearAccel].maxRange = list[Accelerometer].maxRange; 499249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[LinearAccel].minDelay = 5000; 499349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 499449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 499549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 499649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 499749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillSignificantMotion(struct sensor_t *list) 499849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 499949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 500049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 500149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[SignificantMotion].power = list[Accelerometer].power; 500249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[SignificantMotion].resolution = 1; 500349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[SignificantMotion].maxRange = 1; 500449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[SignificantMotion].minDelay = -1; 500549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 500649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 500749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 500849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::fillScreenOrientation(struct sensor_t *list) 500949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 501049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 501149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 501249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[NumSensors].power = list[Accelerometer].power; 501349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[NumSensors].resolution = 1; 501449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[NumSensors].maxRange = 3; 501549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow list[NumSensors].minDelay = 0; 501649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 501749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 501849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 501949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::inv_init_sysfs_attributes(void) 502049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 502149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 502249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 502349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char i = 0; 502449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char sysfs_path[MAX_SYSFS_NAME_LEN]; 50254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow char tbuf[2]; 502649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char *sptr; 502749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char **dptr; 502849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int num; 502949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 503049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(sysfs_path, 0, sizeof(sysfs_path)); 503149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 503249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sysfs_names_ptr = 503349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); 503449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sptr = sysfs_names_ptr; 503549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sptr != NULL) { 503649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dptr = (char**)&mpu; 503749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow do { 503849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *dptr++ = sptr; 503949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(sptr, 0, sizeof(sptr)); 504049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); 504149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } while (++i < MAX_SYSFS_ATTRB); 504249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 504349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:couldn't alloc mem for sysfs paths"); 504449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 504549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 504649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 504749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // get proper (in absolute) IIO path & build MPU's sysfs paths 504849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_sysfs_path(sysfs_path); 504949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 50504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (strcmp(sysfs_path, "") == 0) 505149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 505249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 505349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path)); 505449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.key, "%s%s", sysfs_path, "/key"); 505549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); 505649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length"); 5057cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.master_enable, "%s%s", sysfs_path, "/master_enable"); 505849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); 505949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 506049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, 506149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/scan_elements/in_timestamp_en"); 506249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path, 506349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/scan_elements/in_timestamp_index"); 506449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path, 506549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/scan_elements/in_timestamp_type"); 506649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 506749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware"); 506849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded"); 506949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.dmp_on, "%s%s", sysfs_path, "/dmp_on"); 507049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on"); 507149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.dmp_event_int_on, "%s%s", sysfs_path, "/dmp_event_int_on"); 507249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on"); 507349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 507449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test"); 507549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 507649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); 507749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); 507849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); 507949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix"); 508049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable"); 508149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale"); 508249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable"); 50834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate"); 508449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 508549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable"); 508649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); 508749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix"); 508849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable"); 50894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate"); 509049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 509149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifndef THIRD_PARTY_ACCEL //MPUxxxx 509249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); 509349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 509449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // DMP uses these values 509549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias"); 509649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias"); 509749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias"); 5098cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5099cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro // MPU uses these values 5100cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.in_accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset"); 5101cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.in_accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset"); 5102cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.in_accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset"); 5103cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.in_accel_self_test_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale"); 510449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 510549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 510649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // DMP uses these bias values 510749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias"); 510849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias"); 510949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias"); 511049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 511149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // MPU uses these bias values 511249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset"); 511349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset"); 511449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset"); 51154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale"); 511649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 511749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on 51184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate"); 51194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 512049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on"); 51214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate"); 51224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 512349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on"); 51244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate"); 51254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 5126cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.six_axis_q_value, "%s%s", sysfs_path, "/six_axes_q_value"); 5127cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 51284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on"); 51294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on"); 513049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 513149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, 513249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/display_orientation_on"); 513349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, 513449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/event_display_orientation"); 513549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 513649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.event_smd, "%s%s", sysfs_path, 513749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/event_smd"); 513849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.smd_enable, "%s%s", sysfs_path, 513949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/smd_enable"); 514049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path, 514149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/smd_delay_threshold"); 514249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path, 514349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/smd_delay_threshold2"); 514449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.smd_threshold, "%s%s", sysfs_path, 514549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/smd_threshold"); 514649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path, 514749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/batchmode_timeout"); 514849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path, 514949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/batchmode_wake_fifo_full_on"); 5150cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.flush_batch, "%s%s", sysfs_path, 5151cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "/flush_batch"); 515249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.pedometer_on, "%s%s", sysfs_path, 515349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/pedometer_on"); 515449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path, 515549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/pedometer_int_on"); 515649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.event_pedometer, "%s%s", sysfs_path, 515749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/event_pedometer"); 515849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(mpu.pedometer_steps, "%s%s", sysfs_path, 515949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "/pedometer_steps"); 5160cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro sprintf(mpu.motion_lpa_on, "%s%s", sysfs_path, 5161cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "/motion_lpa_on"); 516249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 516349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 516449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 516549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowbool MPLSensor::isMpuNonDmp(void) 516649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5167cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 516849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) 516949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return true; 517049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else 517149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return false; 517249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 517349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 517449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::isLowPowerQuatEnabled(void) 517549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5176cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 517749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_LP_QUAT_FEAT 517849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return !isMpuNonDmp(); 517949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 518049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 518149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 518249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 518349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 518449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::isDmpDisplayOrientationOn(void) 518549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5186cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 518749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT 518849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isMpuNonDmp()) 518949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 519049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 1; 519149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#else 519249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 519349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#endif 519449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 519549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 519649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* these functions can be consolidated 519749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowwith inv_convert_to_body_with_scale */ 519849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::getCompassBias() 519949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 520049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 520149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 520249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 520349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long bias[3]; 520449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compassBias[3]; 520549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short orient; 520649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow signed char orientMtx[9]; 520749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassSensor->getOrientationMatrix(orientMtx); 520849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow orient = inv_orientation_matrix_to_scalar(orientMtx); 520949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Get Values from MPL */ 521049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_compass_bias(bias); 521149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); 521249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_convert_to_body(orient, bias, compassBias); 52134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]); 52144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]); 521549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long compassSensitivity = inv_get_compass_sensitivity(); 521649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (compassSensitivity == 0) { 521749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow compassSensitivity = mCompassScale; 521849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 521949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for(int i=0; i<3; i++) { 522049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* convert to uT */ 522149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float temp = (float) compassSensitivity / (1L << 30); 522249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f); 522349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 522449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 522549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 522649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 522749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 522849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::getFactoryGyroBias() 522949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 523049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 523149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 523249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //TODO: mllite needs to add this function 523349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //if(inv_factory_bias_available) { 523449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Get Values from MPL */ 523549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_gyro_bias(mFactoryGyroBias); 52364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]); 523749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFactoryGyroBiasAvailable = true; 523849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //} 523949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 524049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 524149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 524249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5243cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/* set bias from factory cal file to MPU offset (in chip frame) 5244cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro x = values store in cal file --> (v/1000 * 2^16 / (2000/250)) 5245cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale 5246cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro i.e. self test default scale = 250 5247cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro gyro scale default to = 2000 5248cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro offset scale = 4 //as spec by hardware 5249cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro offset = x/2^16 * (8) * (-1) / (4) 52504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow*/ 52514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 525249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::setFactoryGyroBias() 525349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 525449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 52554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int scaleRatio = mGyroScale / mGyroSelfTestScale; 52564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int offsetScale = 4; 5257cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio); 5258cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale); 525949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 526049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Write to Driver */ 52614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 52624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale), 52634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mpu.in_gyro_x_offset, getTimestamp()); 52644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(write_attribute_sensor_continuous(gyro_x_offset_fd, 52654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0) 526649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 526749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to gyro_x_offset"); 526849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 526949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 52704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 52714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale), 52724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mpu.in_gyro_y_offset, getTimestamp()); 52734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(write_attribute_sensor_continuous(gyro_y_offset_fd, 52744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0) 527549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 527649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to gyro_y_offset"); 527749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 527849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 52794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 52804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale), 52814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mpu.in_gyro_z_offset, getTimestamp()); 52824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(write_attribute_sensor_continuous(gyro_z_offset_fd, 52834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0) 528449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow { 528549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to gyro_z_offset"); 528649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 528749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 528849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFactoryGyroBiasAvailable = false; 528949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied"); 529049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 529149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 529249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 529349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 529449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* these functions can be consolidated 529549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowwith inv_convert_to_body_with_scale */ 529649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::getGyroBias() 529749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 529849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 529949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 530049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long *temp = NULL; 530149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long chipBias[3]; 530249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long bias[3]; 530349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned short orient; 530449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 530549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Get Values from MPL */ 530649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_mpl_gyro_bias(mGyroChipBias, temp); 530749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow orient = inv_orientation_matrix_to_scalar(mGyroOrientation); 530849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); 530949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_convert_to_body(orient, mGyroChipBias, bias); 53104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]); 53114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]); 531249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gyroSensitivity = inv_get_gyro_sensitivity(); 531349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(gyroSensitivity == 0) { 531449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyroSensitivity = mGyroScale; 531549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 53164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 531749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* scale and convert to rad */ 531849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for(int i=0; i<3; i++) { 531949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow float temp = (float) gyroSensitivity / (1L << 30); 532049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI); 532149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mGyroBias[i] != 0) 532249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroBiasAvailable = true; 532349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 532449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 532549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 532649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 532749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 532849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::setGyroBias() 532949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 533049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 533149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 533249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mGyroBiasAvailable == false) 533349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 533449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 533549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long bias[3]; 533649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long gyroSensitivity = inv_get_gyro_sensitivity(); 533749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 533849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(gyroSensitivity == 0) { 533949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow gyroSensitivity = mGyroScale; 534049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 53414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 53424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_get_gyro_bias_dmp_units(bias); 534349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 534449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Write to Driver */ 534549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 534649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp()); 5347cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0) { 534849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to gyro_x_dmp_bias"); 534949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 535049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 535149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 535249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp()); 5353cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0) { 535449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to gyro_y_dmp_bias"); 535549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 535649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 535749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 535849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp()); 5359cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0) { 536049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to gyro_z_dmp_bias"); 536149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 536249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 536349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroBiasApplied = true; 536449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mGyroBiasAvailable = false; 536549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied"); 536649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 536749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 536849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 536949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 537049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::getFactoryAccelBias() 537149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 537249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 537349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 53744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow long temp; 537549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 53764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* Get Values from MPL */ 5377cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_accel_bias(mFactoryAccelBias); 5378cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]); 53794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mFactoryAccelBiasAvailable = true; 5380cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 538149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 538249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 538349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 538449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::setFactoryAccelBias() 538549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5386cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 5387cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 538849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mFactoryAccelBiasAvailable == false) 538949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 539049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5391cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* add scaling here - depends on self test parameters */ 5392cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int scaleRatio = mAccelScale / mAccelSelfTestScale; 5393cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int offsetScale = 16; 5394cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro long tempBias; 5395cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5396cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio); 5397cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale); 53984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 539949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Write to Driver */ 5400cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempBias = -mFactoryAccelBias[0] / 65536.f * scaleRatio / offsetScale; 5401cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 5402cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempBias, mpu.in_accel_x_offset, getTimestamp()); 5403cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_x_offset_fd, tempBias) < 0) { 540449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to accel_x_offset"); 540549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 540649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5407cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempBias = -mFactoryAccelBias[1] / 65536.f * scaleRatio / offsetScale; 5408cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 5409cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempBias, mpu.in_accel_y_offset, getTimestamp()); 5410cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_y_offset_fd, tempBias) < 0) { 541149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to accel_y_offset"); 541249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 541349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5414cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempBias = -mFactoryAccelBias[2] / 65536.f * scaleRatio / offsetScale; 5415cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 5416cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro tempBias, mpu.in_accel_z_offset, getTimestamp()); 5417cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_z_offset_fd, tempBias) < 0) { 541849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to accel_z_offset"); 541949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 542049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 542149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFactoryAccelBiasAvailable = false; 542249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied"); 542349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 542449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 542549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 542649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 542749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::getAccelBias() 542849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 542949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 54304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow long temp; 543149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 54324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* Get Values from MPL */ 5433cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_get_mpl_accel_bias(mAccelBias, &temp); 5434cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld", 5435cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mAccelBias[0], mAccelBias[1], mAccelBias[2]); 54364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mAccelBiasAvailable = true; 543749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 543849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 543949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 544049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5441cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro/* set accel bias obtained from MPL 5442cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro bias is scaled by 65536 from MPL 5443cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro DMP expects: bias * 536870912 / 2^30 = bias / 2 (in body frame) 5444cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro*/ 544549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::setAccelBias() 544649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 544749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 544849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5449cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(mAccelBiasAvailable == false) { 5450cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL: setAccelBias - accel bias not available"); 545149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 5452cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 545349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5454cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* 54554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow long bias[3]; 54564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow unsigned short orient = inv_orientation_matrix_to_scalar(mAccelOrientation); 54574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_convert_to_body(orient, mAccelBias, bias); 5458cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro */ 54594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 5460cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* write to driver */ 546149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 5462cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (long) (mAccelBias[0] / 65536.f / 2), 5463cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mpu.in_accel_x_dmp_bias, getTimestamp()); 5464cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous( 5465cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0) { 546649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to accel_x_dmp_bias"); 546749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 546849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 546949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 5470cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (long)(mAccelBias[1] / 65536.f / 2), 5471cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mpu.in_accel_y_dmp_bias, getTimestamp()); 5472cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous( 5473cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0) { 547449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to accel_y_dmp_bias"); 547549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 547649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 547749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)", 5478cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (long)(mAccelBias[2] / 65536 / 2), 5479cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mpu.in_accel_z_dmp_bias, getTimestamp()); 5480cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous( 5481cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0) { 548249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:Error writing to accel_z_dmp_bias"); 548349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 548449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5485cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 548649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelBiasAvailable = false; 548749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mAccelBiasApplied = true; 548849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied"); 548949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 549049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 549149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 549249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 549349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::isCompassDisabled(void) 549449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5495cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 549649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) { 549749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used."); 549849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 1; 549949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 550049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 550149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 550249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5503cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::checkBatchEnabled(void) 5504cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 5505cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 5506cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return ((mFeatureActiveMask & INV_DMP_BATCH_MODE)? 1:0); 5507cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 5508cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 55094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* precondition: framework disallows this case, ie enable continuous sensor, */ 55104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* and enable batch sensor */ 55114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */ 55124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* or any other sensors */ 5513cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#define DEBUG_BATCHING (0) 551449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout) 551549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 551649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 551749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 551849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 551949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 552049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isMpuNonDmp()) 552149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 552249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 552349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Enables batch mode and sets timeout for the given sensor */ 552449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */ 552549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow bool dryRun = false; 552649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow android::String8 sname; 552749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int what = -1; 552849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int enabled_sensors = mEnabled; 552949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int batchMode = timeout > 0 ? 1 : 0; 553049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 553149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE, 553249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld", 553349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow handle, flags, period_ns, timeout); 553449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 553549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(flags & (1 << SENSORS_BATCH_DRY_RUN)) { 553649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dryRun = true; 553749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(PROCESS_VERBOSE, 553849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN); 553949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 554049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 554149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow getHandle(handle, what, sname); 554249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(uint32_t(what) >= NumSensors) { 554349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:batch sensors %d not found", what); 554449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -EINVAL; 554549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 554649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5547cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro switch (what) { 5548cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case Gyro: 5549cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case RawGyro: 5550cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case Accelerometer: 5551cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case MagneticField: 5552cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case RawMagneticField: 5553cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case Pressure: 5554cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case GameRotationVector: 5555cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro case StepDetector: 5556cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle); 5557cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro break; 5558cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro default: 5559cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (timeout > 0) { 5560cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, "sensor (handle %d) is not supported in batch mode", handle); 5561cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return -EINVAL; 5562cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5563cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5564cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 556549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int tempBatch = 0; 556649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (timeout > 0) { 556749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tempBatch = mBatchEnabled | (1 << what); 556849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 556949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow tempBatch = mBatchEnabled & ~(1 << what); 557049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 557149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 557249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!computeBatchSensorMask(mEnabled, tempBatch)) { 557349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow batchMode = 0; 55744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 55754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow batchMode = 1; 557649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 55774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 557849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* get maximum possible bytes to batch per sample */ 557949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* get minimum delay for each requested sensor */ 558049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ssize_t nBytes = 0; 558149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t wanted = 1000000000LL, ns = 0; 558249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int64_t timeoutInMs = 0; 558349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (int i = 0; i < NumSensors; i++) { 558449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (batchMode == 1) { 55854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ns = mBatchDelays[i]; 558649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE, 55874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns); 558849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // take the min delay ==> max rate 558949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow wanted = (ns < wanted) ? ns : wanted; 559049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (i <= RawMagneticField) { 559149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow nBytes += 8; 559249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 559349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (i == Pressure) { 559449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow nBytes += 6; 559549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 559649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if ((i == StepDetector) || (i == GameRotationVector)) { 559749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow nBytes += 16; 559849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 559949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 560049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 560149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 560249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* check if we can support issuing interrupt before FIFO fills-up */ 560349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* in the given timeout. */ 5604cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) { 56054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported"); 56064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return -EINVAL; 56074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* provide messge if it exceeds hardware capability 560849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (nSamples * nBytes >= 1024) { 560949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:batch - timeout - configuration is not supported, " 561049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow "cannot provide requested amount of buffering (%lld ms)", 561149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timeout / 1000000LL); 56124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow }*/ 561349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 561449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 561549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(dryRun == true) 561649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 561749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 561849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* starting from code below, we will modify hardware */ 561949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* first edit global batch mode mask */ 562049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (!timeout) { 562249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mBatchEnabled &= ~(1 << what); 562349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mBatchDelays[what] = 1000000000L; 5624cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mBatchTimeouts[what] = 100000000000LL; 562549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 562649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mBatchEnabled |= (1 << what); 562749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mBatchDelays[what] = period_ns; 56284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mBatchTimeouts[what] = timeout; 562949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 56304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 5631cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* set mask used by other functions */ 5632cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mOldBatchEnabledMask = batchMode; 5633cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 56344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* For these sensors, switch to different data output */ 56354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* These steps should be optimized at some point */ 56364a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int featureMask = computeBatchDataOutput(); 56374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 56384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d", 56394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow batchMode, featureMask, mEnabled); 564049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (DEBUG_BATCHING || EXTRA_VERBOSE) { 564149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled); 56424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow for (int d = 0; d < NumSensors; d++) { 56434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld", 56444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d], 56454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mBatchDelays[d]); 56464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 56474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 56484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 56494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* take the minimum batchmode timeout */ 56504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (batchMode == 1) { 5651cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int64_t tempTimeout = 100000000000LL; 5652cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro for (int i = 0; i < NumSensors; i++) { 56534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mEnabled & (1 << i) && mBatchEnabled & (1 << i)) || 56544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (((featureMask & INV_DMP_PED_STANDALONE) && (mBatchEnabled & (1 << StepDetector))))) { 56554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "i=%d, timeout=%lld", i, mBatchTimeouts[i]); 56564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow ns = mBatchTimeouts[i]; 56574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempTimeout = (ns < tempTimeout) ? ns : tempTimeout; 56584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 565949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 56604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeout = tempTimeout; 56614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* Convert ns to millisecond */ 56624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeoutInMs = timeout / 1000000; 56634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 56644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* remember last timeout value */ 56654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mBatchTimeoutInMs = timeoutInMs; 56664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 56674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* TODO: Calculate nSamples */ 56684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int nSamples = 0; 56694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow nSamples = (unsigned long)( 56704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (1000000000.f / wanted) * ((float)timeout / 1000000000.f)); 56714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 56724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeoutInMs = 0; 567349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 56744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 56754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(DEBUG_BATCHING || EXTRA_VERBOSE, 56764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:batch - timeout - timeout=%lld ns, timeoutInMs=%lld, delay=%lld ns", 56774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow timeout, timeoutInMs, wanted); 567849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 567949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // reset master enable 568049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 568149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 568249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 568349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 568449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 56854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* case for Ped standalone */ 56864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) && 56874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mFeatureActiveMask & INV_DMP_PEDOMETER)) { 56884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGI("ID_P only = 0x%x", mBatchEnabled); 56894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedQuaternion(0); 56904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedStandalone(1); 56914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 56924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedStandalone(0); 569349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (featureMask & INV_DMP_PED_QUATERNION) { 569449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(0); 569549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enablePedQuaternion(1); 569649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 569749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 56984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 56994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* case for Ped Quaternion */ 57004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) && 57014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mEnabled & (1 << GameRotationVector)) && 57024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mFeatureActiveMask & INV_DMP_PEDOMETER)) { 57034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGI("ID_P and GRV or ALL = 0x%x", mBatchEnabled); 57044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGI("ID_P is enabled for batching, PED quat will be automatically enabled"); 57054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enableLPQuaternion(0); 57064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedQuaternion(1); 57074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 57084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted = mBatchDelays[GameRotationVector]; 57094a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* set pedq rate */ 57104a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 57114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int(1000000000.f / wanted), mpu.ped_q_rate, 57124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 57134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted); 57144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 57154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted); 57164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if (!(featureMask & INV_DMP_PED_STANDALONE)){ 5717cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "Toggle back to normal 6 axis"); 5718cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mEnabled & (1 << GameRotationVector) && checkLPQRateSupported()) { 57194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enableLPQuaternion(1); 57204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 57214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedQuaternion(0); 572249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 572349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 57244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* case for Ped indicator */ 57254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) { 57264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedIndicator(1); 57274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 57284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enablePedIndicator(0); 57294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 57304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 57314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* case for Six Axis Quaternion */ 57324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) && 57334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow (mEnabled & (1 << GameRotationVector))) { 573449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI("GRV = 0x%x", mBatchEnabled); 573549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(0); 573649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enable6AxisQuaternion(1); 5737cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (what == GameRotationVector) { 5738cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro setInitial6QuatValue(); 5739cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 57404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 57414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted = mBatchDelays[GameRotationVector]; 57424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* set sixaxis rate */ 57434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 57444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int(1000000000.f / wanted), mpu.six_axis_q_rate, 57454a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 57464a28f9c897c46c42a255823f7e307169a828a025Rosa Chow write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted); 57474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 5748cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted); 57494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else if (!(featureMask & INV_DMP_PED_QUATERNION)){ 5750cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "Toggle back to normal 6 axis"); 5751cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (mEnabled & (1 << GameRotationVector) && checkLPQRateSupported()) { 575249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enableLPQuaternion(1); 575349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 575449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow enable6AxisQuaternion(0); 57554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 57564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow enable6AxisQuaternion(0); 575749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 57584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 57594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* TODO: This may make a come back some day */ 576049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* write not to overflow hardware FIFO if flag is set */ 57614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) { 576249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 576349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.batchmode_wake_fifo_full_on, getTimestamp()); 576449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) { 576549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on"); 576649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 57674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow }*/ 576849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 576949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* write required timeout to sysfs */ 577049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)", 577149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow timeoutInMs, mpu.batchmode_timeout, getTimestamp()); 577249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) { 577349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write batchmode_timeout"); 577449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 577549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 577649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (batchMode == 1) { 577749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable DMP 577849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(1); 577949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 578049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 578149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 57824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow // set batch rates 57834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (setBatchDataRates() < 0) { 57844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set batch data rates"); 57854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 578649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // default fifo rate to 200Hz 578749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 578849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 200, mpu.gyro_fifo_rate, getTimestamp()); 578949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) { 579049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 579149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't set DMP rate to 200Hz"); 579249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 579349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 579449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 57954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mFeatureActiveMask == 0) && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK)) { 579649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP 579749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(0); 579849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) { 579949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 580049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5801cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /*if (resetDataRates() < 0) { 5802cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:ERR can't reset output rate back to original setting"); 5803cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro }*/ 58044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 58054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* reset sensor rate */ 5806cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (resetDataRates() < 0) { 58074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't reset output rate back to original setting"); 5808cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 580949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5810cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (enabled_sensors || mFeatureActiveMask) { 581149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(1); 581249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 581349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 581449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 581549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5816cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::flush(int handle) 581749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 581849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 581949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5820cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int res = 0; 5821cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro android::String8 sname; 5822cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int what = -1; 5823cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5824cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* TODO: return zero if no data in FIFO */ 5825cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getHandle(handle, what, sname); 5826cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (uint32_t(what) >= NumSensors) { 5827cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:flush - what=%d is invalid", what); 5828cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return -EINVAL; 5829cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5830cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5831cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (((what != StepDetector) && (!(mEnabled & (1 << what)))) || 5832cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro ((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) { 5833cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL: flush - sensor %s not enabled", sname.string()); 5834cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return -EINVAL; 5835cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5836cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(!(mBatchEnabled & (1 << what))) { 5837cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:flush - batch mode not enabled for sensor %s", sname.string()); 5838cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0; 5839cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5840cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, "HAL:flush - sensor %s (handle %d)", sname.string(), handle); 5841cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5842cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /*write sysfs */ 5843cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", 5844cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mpu.flush_batch, getTimestamp()); 5845cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5846cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro read_sysfs_int(mpu.flush_batch, &res); 5847cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (res < 0) { 5848cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:ERR can't read flush_batch"); 5849cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return -1; 5850cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5851cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5852cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (res == 0) { 5853cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGI("HAL: flush - no data in FIFO"); 5854cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 5855cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5856cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushEnabled = handle; 5857cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushEnabled=%d", mFlushEnabled); 5858cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5859cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mFlushBatchSet = 0; 5860cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return res; 5861cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 5862cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5863cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::computeBatchDataOutput() 5864cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 5865cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 5866cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 586749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int featureMask = 0; 586849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mBatchEnabled == 0) 586949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0;//h 587049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5871cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro uint32_t hardwareSensorMask = (1 << Gyro) 5872cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << RawGyro) 5873cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << Accelerometer) 5874cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << MagneticField) 5875cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << RawMagneticField) 5876cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro | (1 << Pressure); 5877cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x", 5878cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro hardwareSensorMask, mBatchEnabled); 58794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 58804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mBatchEnabled & (1 << StepDetector)) { 58814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mBatchEnabled & (1 << GameRotationVector)) { 58824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if ((mBatchEnabled & hardwareSensorMask)) { 58834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_6AXIS_QUATERNION;//a 58844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_PED_INDICATOR; 58854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 58864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_PED_QUATERNION; //b 58874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_PED_INDICATOR; //always piggy back a bit 58884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 58894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 58904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mBatchEnabled & hardwareSensorMask) { 58914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_PED_INDICATOR; //c 58924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 58934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_PED_STANDALONE; //d 58944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow featureMask |= INV_DMP_PED_INDICATOR; //required for standalone 58954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 589649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5897cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else if (mBatchEnabled & (1 << GameRotationVector)) { 5898cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro featureMask |= INV_DMP_6AXIS_QUATERNION; //e,f 58994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } else { 5900cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 5901cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask); 59024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return 0; //g 590349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 59044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 5905cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, 5906cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask); 590749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return featureMask; 590849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 590949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 591049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getDmpPedometerFd() 591149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5912cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 5913cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "getDmpPedometerFd returning %d", dmp_pedometer_fd); 591449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return dmp_pedometer_fd; 591549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 591649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 59174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* @param [in] : outputType = 1 --derive from ped_q */ 59184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* outputType = 0 --derive from IRQ */ 591949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count, 592049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int32_t id, int32_t type, int outputType) 592149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 592249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 592349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 592449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 592549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char dummy[4]; 592649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *fp; 592749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint64_t stepCount = 0; 592849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int numEventReceived = 0; 592949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 0; 593049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 593149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if((mDmpStepCountEnabled || mDmpPedometerEnabled) && count > 0) { 593249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* handles return event */ 593349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors_event_t temp; 593449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 593549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI_IF(EXTRA_VERBOSE, "HAL: Read Pedometer Event ID=%d", id); 593649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.version = sizeof(sensors_event_t); 593749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.sensor = id; 593849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.type = type; 593949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.acceleration.status 594049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow = SENSOR_STATUS_UNRELIABLE; 594149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* sensors.h specified to return 1.0 */ 594249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(id == ID_P) { 594349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.data[0] = 1; 594449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.data[1] = 0.f; 594549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.data[2] = 0.f; 594649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 594749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fp = fopen(mpu.pedometer_steps, "r"); 594849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (fp == NULL) { 594949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:cannot open pedometer_steps"); 595049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else{ 59514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (fscanf(fp, "%lld\n", &stepCount) < 0 || fclose(fp) < 0) { 59524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:cannot read pedometer_steps"); 5953cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0; 59544a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 595549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 595649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* return onChange only*/ 595749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (stepCount == mLastStepCount) { 595849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 595949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 5960cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* TODO: framework needs to support 64-bit */ 5961cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#ifdef TESTING 5962cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro temp.data[0] = (float)stepCount; 5963cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#else 5964cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro temp.u64.step_counter = stepCount; 5965cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#endif 596649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mLastStepCount = stepCount; 596749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 596849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 596949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!outputType) { 597049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct timespec ts; 597149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow clock_gettime(CLOCK_MONOTONIC, &ts) ; 597249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow temp.timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec; 597349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 597449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 597549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *data++ = temp; 597649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count--; 597749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow numEventReceived++; 597849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 597949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 598049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!outputType) { 598149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // read dummy data per driver's request 598249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // only required if actual irq is issued 598349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow read(dmp_pedometer_fd, dummy, 4); 598449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 598549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 1; 598649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 598749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 598849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return numEventReceived; 598949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 599049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 599149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::getDmpSignificantMotionFd() 599249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5993cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 5994cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 5995cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "getDmpSignificantMotionFd returning %d", 599649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dmp_sign_motion_fd); 599749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return dmp_sign_motion_fd; 599849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 599949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6000cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaroint MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) 6001cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 600249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 600349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 600449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 600549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char dummy[4]; 600649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int significantMotion; 600749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow FILE *fp; 600849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int sensors = mEnabled; 600949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int numEventReceived = 0; 601049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = 0; 601149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 601249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Technically this step is not necessary for now */ 601349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* In the future, we may have meaningful values */ 601449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow fp = fopen(mpu.event_smd, "r"); 601549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (fp == NULL) { 601649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:cannot open event_smd"); 601749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return 0; 6018cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 6019cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fscanf(fp, "%d\n", &significantMotion) < 0 || fclose(fp) < 0) { 6020cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:cannot read event_smd"); 6021cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 602249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 602349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 602449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(mDmpSignificantMotionEnabled && count > 0) { 602549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* By implementation, smd is disabled once an event is triggered */ 602649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sensors_event_t temp; 602749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 602849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow /* Handles return event */ 602949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI("HAL: SMD detected"); 603049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int update = smHandler(&temp); 603149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (update && count > 0) { 603249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *data++ = temp; 603349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow count--; 603449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow numEventReceived++; 603549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mDmpSignificantMotionEnabled = 0; 603649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION; 6037cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#if 0 6038cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(mFeatureActiveMask == 0 || sensors == 0) { 603949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGI("dmp off"); 604049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP 604149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(0); 604249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(0); 604349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 604449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 604549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 604649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel engine 604749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask 604849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_ACCEL)) { 604949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(0); 605049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 605149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 605249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 605349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 6054cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro#endif 605549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 605649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 605749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 605849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // read dummy data per driver's request 605949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow read(dmp_sign_motion_fd, dummy, 4); 606049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 606149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return numEventReceived; 606249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 606349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 606449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::enableDmpSignificantMotion(int en) 606549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 606649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 606749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6068cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int res = 0; 606949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int enabled_sensors = mEnabled; 607049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 607149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (isMpuNonDmp()) 607249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 607349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 607449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // reset master enable 607549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(0); 607649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 607749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 607849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 607949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow //Toggle significant montion detection 608049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(en) { 6081cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Enabling Significant Motion"); 608249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 608349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 1, mpu.smd_enable, getTimestamp()); 608449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.smd_enable, 1) < 0) { 608549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't write DMP smd_enable"); 608649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; //Indicate an err 608749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 608849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 608949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable DMP 609049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(1); 609149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 609249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 609349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 609449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // set DMP rate to 200Hz 609549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 609649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 200, mpu.accel_fifo_rate, getTimestamp()); 609749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { 609849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 60994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:ERR can't set rate to 200Hz"); 610049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 610149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 610249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 610349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // enable accel engine 610449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(1); 610549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 610649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 610749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 610849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel FIFO 610949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { 611049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = turnOffAccelFifo(); 611149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 611249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 611349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 611449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION; 611549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 611649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow else { 6117cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "HAL:Disabling Significant Motion"); 611849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 611949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 0, mpu.smd_enable, getTimestamp()); 612049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.smd_enable, 0) < 0) { 612149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR write DMP smd_enable"); 612249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 612349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION; 612449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable DMP 612549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (mFeatureActiveMask == 0) { 612649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = onDmp(0); 612749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 612849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 612949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 613049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 613149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // disable accel engine 613249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (!(mLocalSensorMask & mMasterSensorMask 613349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow & INV_THREE_AXIS_ACCEL)) { 613449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = enableAccel(0); 613549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res < 0) 613649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 613749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 613849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 613949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(enabled_sensors) { 614049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 614149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow en, mpu.dmp_event_int_on, getTimestamp()); 614249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { 614349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = -1; 614449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow LOGE("HAL:ERR can't enable DMP event interrupt"); 614549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 614649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 614749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 614849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if(en || enabled_sensors || mFeatureActiveMask) { 614949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = masterEnable(1); 615049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 615149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 615249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 615349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6154cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccarovoid MPLSensor::setInitial6QuatValue() 6155cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro{ 6156cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 6157cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6158cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (!mInitial6QuatValueAvailable) 6159cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return; 6160cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6161cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* convert to unsigned char array */ 6162cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro size_t length = 16; 6163cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro unsigned char quat[16]; 6164cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro convert_long_to_hex_char(mInitial6QuatValue, quat, 4); 6165cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6166cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* write to sysfs */ 6167cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(EXTRA_VERBOSE, "HAL:six axis q value: %s", mpu.six_axis_q_value); 6168cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro FILE* fptr = fopen(mpu.six_axis_q_value, "w"); 6169cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(fptr == NULL) { 6170cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:could not open six_axis_q_value"); 6171cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 6172cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (fwrite(quat, 1, length, fptr) != length || fclose(fptr) < 0) { 6173cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:write six axis q value failed"); 6174cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } else { 6175cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGI("!!!! write dmp 6 axis !!!"); 6176cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mInitial6QuatValueAvailable = 0; 6177cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6178cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6179cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6180cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return; 6181cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro} 618249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowint MPLSensor::writeSignificantMotionParams(bool toggleEnable, 618349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t delayThreshold1, 618449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t delayThreshold2, 618549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t motionThreshold) 618649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 6187cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 6188cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 618949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int res = 0; 619049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 619149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Turn off enable 619249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (toggleEnable) { 619349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(0); 619449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 619549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 619649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Write supplied values 61974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 61984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow delayThreshold1, mpu.smd_delay_threshold, getTimestamp()); 619949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1); 620049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res == 0) { 62014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 62024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow delayThreshold2, mpu.smd_delay_threshold2, getTimestamp()); 620349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2); 620449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 620549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (res == 0) { 62064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 62074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow motionThreshold, mpu.smd_threshold, getTimestamp()); 620849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow res = write_sysfs_int(mpu.smd_threshold, motionThreshold); 620949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 621049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 621149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Turn on enable 621249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (toggleEnable) { 621349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow masterEnable(1); 621449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 621549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return res; 621649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 621749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 62184a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* set batch data rate */ 62194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* this function should be optimized */ 62204a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::setBatchDataRates() 62214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 62224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow VFUNC_LOG; 62234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int res = 0; 62254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int tempFd = -1; 62264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t gyroRate; 62284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t accelRate; 62294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t compassRate; 62304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t pressureRate; 6231cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int64_t quatRate; 62324a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62334a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int mplGyroRate; 62344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int mplAccelRate; 62354a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int mplCompassRate; 6236cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro int mplQuatRate; 6237cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 62384a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#ifdef ENABLE_MULTI_RATE 62394a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* take care of case where only one type of gyro sensors or compass sensors is turned on */ 62404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow gyroRate = mBatchDelays[Gyro] < mBatchDelays[RawGyro] ? mBatchDelays[Gyro] : mBatchDelays[RawGyro]; 62414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow accelRate = mBatchDelays[Accelerometer]; 62424a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = mBatchDelays[MagneticField] < mBatchDelays[RawMagneticField] ? mBatchDelays[MagneticField] : mBatchDelays[RawMagneticField]; 62434a28f9c897c46c42a255823f7e307169a828a025Rosa Chow pressureRate = mBatchDelays[Pressure]; 62444a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6245cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) || 6246cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) { 6247cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro quatRate = mBatchDelays[GameRotationVector]; 6248cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro mplQuatRate = (int) quatRate / 1000LL; 6249cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro inv_set_quat_sample_rate(mplQuatRate); 6250cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, 6251cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL quat sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate, 6252cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 1000000000.f / quatRate ); 6253cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro getDmpRate(&quatRate); 6254cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6255cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 62564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mplGyroRate = (int) gyroRate / 1000LL; 62574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mplAccelRate = (int) accelRate / 1000LL; 62584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mplCompassRate = (int) compassRate / 1000LL; 62594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* set rate in MPL */ 62614a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* compass can only do 100Hz max */ 62624a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_gyro_sample_rate(mplGyroRate); 62634a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_accel_sample_rate(mplAccelRate); 62644a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_compass_sample_rate(mplCompassRate); 62654a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62664a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 62674a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate); 62684a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 62694a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate); 62704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(PROCESS_VERBOSE, 62714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate); 62724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#else 62744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* search the minimum delay requested across all enabled sensors */ 62754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t wanted = 1000000000LL; 6276cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro for (int i = 0; i < NumSensors; i++) { 62774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mBatchEnabled & (1 << i)) { 62784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t ns = mBatchDelays[i]; 62794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted = wanted < ns ? wanted : ns; 62804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 62814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 62824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow gyroRate = wanted; 62834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow accelRate = wanted; 62844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = wanted; 62854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow pressureRate = wanted; 62864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow#endif 62874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 62894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / gyroRate, mpu.gyro_rate, 62904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 62914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.gyro_rate, O_RDWR); 62924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate); 62934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(res < 0) { 62944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:GYRO update delay error"); 62954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 62964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 62974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 62984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / accelRate, mpu.accel_rate, 62994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 63004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.accel_rate, O_RDWR); 63014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / accelRate); 63024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 63034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (compassRate < mCompassSensor->getMinDelay() * 1000LL) { 63054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = mCompassSensor->getMinDelay() * 1000LL; 63064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 63074a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCompassSensor->setDelay(ID_M, compassRate); 6308cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6309cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro pressureRate = mDelays[Pressure]; 6310cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 63114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mPressureSensor->setDelay(ID_PS, pressureRate); 63124a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 63144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 63154a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63164a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* Set sensor rate */ 63174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow/* this function should be optimized */ 63184a28f9c897c46c42a255823f7e307169a828a025Rosa Chowint MPLSensor::resetDataRates() 63194a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 63204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow VFUNC_LOG; 63214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63224a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int res = 0; 63234a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int tempFd = -1; 63244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t wanted = 1000000000LL; 63254a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63264a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t gyroRate; 63274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t accelRate; 63284a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t compassRate; 63294a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t pressureRate; 63304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6331cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* TODO: support multi-rate in dmp off mode */ 6332cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6333cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if (!mEnabled) { 6334cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "skip resetDataRates"); 6335cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0; 6336cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6337cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* search the minimum delay requested across all enabled sensors */ 6338cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro /* skip setting rates if it is not changed */ 6339cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro for (int i = 0; i < NumSensors; i++) { 63404a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (mEnabled & (1 << i)) { 63414a28f9c897c46c42a255823f7e307169a828a025Rosa Chow int64_t ns = mDelays[i]; 6342cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if ((wanted == ns) && (i != Pressure)) { 6343cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "skip resetDataRates"); 6344cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro return 0; 6345cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6346cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(ENG_VERBOSE, "resetDataRates - mDelays[%d]=%lld", i, mDelays[i]); 63474a28f9c897c46c42a255823f7e307169a828a025Rosa Chow wanted = wanted < ns ? wanted : ns; 63484a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 63494a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 63504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6351cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro gyroRate = wanted; 6352cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accelRate = wanted; 6353cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro compassRate = wanted; 6354cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro pressureRate = wanted; 63554a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63564a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* set mpl data rate */ 63574a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_gyro_sample_rate((int)gyroRate/1000LL); 63584a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_accel_sample_rate((int)accelRate/1000LL); 63594a28f9c897c46c42a255823f7e307169a828a025Rosa Chow inv_set_compass_sample_rate((int)compassRate/1000LL); 63604a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6361cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, 6362cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz", 6363cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro gyroRate/1000LL, 1000000000.f / gyroRate); 6364cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, 6365cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz", 6366cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro accelRate/1000LL, 1000000000.f / accelRate); 6367cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGV_IF(PROCESS_VERBOSE, 6368cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro "HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz", 6369cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro compassRate/1000LL, 1000000000.f / compassRate); 63704a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63714a28f9c897c46c42a255823f7e307169a828a025Rosa Chow /* reset dmp rate */ 63724a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getDmpRate (&wanted); 63734a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63744a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 63754a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / wanted, mpu.gyro_fifo_rate, 63764a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 63774a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.gyro_fifo_rate, O_RDWR); 63784a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / wanted); 63794a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE_IF(res < 0, "HAL:sampling frequency update delay error"); 63804a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63814a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 63824a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / gyroRate, mpu.gyro_rate, 63834a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 63844a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.gyro_rate, O_RDWR); 63854a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate); 63864a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if(res < 0) { 63874a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:GYRO update delay error"); 63884a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 63894a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63904a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 63914a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 1000000000.f / accelRate, mpu.accel_rate, 63924a28f9c897c46c42a255823f7e307169a828a025Rosa Chow getTimestamp()); 63934a28f9c897c46c42a255823f7e307169a828a025Rosa Chow tempFd = open(mpu.accel_rate, O_RDWR); 63944a28f9c897c46c42a255823f7e307169a828a025Rosa Chow res = write_attribute_sensor(tempFd, 1000000000.f / accelRate); 63954a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 63964a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 63974a28f9c897c46c42a255823f7e307169a828a025Rosa Chow if (compassRate < mCompassSensor->getMinDelay() * 1000LL) { 63984a28f9c897c46c42a255823f7e307169a828a025Rosa Chow compassRate = mCompassSensor->getMinDelay() * 1000LL; 63994a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 64004a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mCompassSensor->setDelay(ID_M, compassRate); 64014a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 64024a28f9c897c46c42a255823f7e307169a828a025Rosa Chow mPressureSensor->setDelay(ID_PS, pressureRate); 64034a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 64044a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return res; 64054a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 64064a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 64074a28f9c897c46c42a255823f7e307169a828a025Rosa Chowvoid MPLSensor::initBias() 64084a28f9c897c46c42a255823f7e307169a828a025Rosa Chow{ 6409cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro VFUNC_LOG; 6410cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 64114a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0"); 6412cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0) { 64134a28f9c897c46c42a255823f7e307169a828a025Rosa Chow LOGE("HAL:Error writing to accel_x_dmp_bias"); 64144a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 6415cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0) { 6416cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to accel_y_dmp_bias"); 64174a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 6418cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0) { 6419cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to accel_z_dmp_bias"); 64204a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 64214a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6422cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_x_offset_fd, 0) < 0) { 6423cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to accel_x_offset"); 64244a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 6425cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_y_offset_fd, 0) < 0) { 6426cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to accel_y_offset"); 64274a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 6428cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(accel_z_offset_fd, 0) < 0) { 6429cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to accel_z_offset"); 64304a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 64314a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 6432cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) { 6433cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to gyro_x_dmp_bias"); 64344a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 6435cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) { 6436cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to gyro_y_dmp_bias"); 64374a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 6438cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) { 6439cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to gyro_z_dmp_bias"); 6440cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6441cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro 6442cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0) { 6443cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to gyro_x_offset"); 6444cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6445cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0) { 6446cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to gyro_y_offset"); 6447cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro } 6448cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0) { 6449cd79002b2edb60b25843e5f4f9a06e768bc1a568Nick Vaccaro LOGE("HAL:Error writing to gyro_z_offset"); 64504a28f9c897c46c42a255823f7e307169a828a025Rosa Chow } 64514a28f9c897c46c42a255823f7e307169a828a025Rosa Chow return; 64524a28f9c897c46c42a255823f7e307169a828a025Rosa Chow} 64534a28f9c897c46c42a255823f7e307169a828a025Rosa Chow 645449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/*TODO: reg_dump in a separate file*/ 645549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid MPLSensor::sys_dump(bool fileMode) 645649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 645749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow VFUNC_LOG; 645849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 645949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char sysfs_path[MAX_SYSFS_NAME_LEN]; 646049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow char scan_element_path[MAX_SYSFS_NAME_LEN]; 646149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 646249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(sysfs_path, 0, sizeof(sysfs_path)); 646349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(scan_element_path, 0, sizeof(scan_element_path)); 646449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_get_sysfs_path(sysfs_path); 646549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements"); 646649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 646749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow read_sysfs_dir(fileMode, sysfs_path); 646849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow read_sysfs_dir(fileMode, scan_element_path); 646949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 647049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow dump_dmp_img("/data/local/read_img.h"); 647149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return; 647249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 6473