1 /* 2 * Copyright (C) 2011 Invensense, Inc. 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 //#define LOG_NDEBUG 0 18 //see also the EXTRA_VERBOSE define, below 19 20 #include <fcntl.h> 21 #include <errno.h> 22 #include <math.h> 23 #include <float.h> 24 #include <poll.h> 25 #include <unistd.h> 26 #include <dirent.h> 27 #include <stdlib.h> 28 #include <sys/select.h> 29 #include <dlfcn.h> 30 #include <pthread.h> 31 32 #include <cutils/log.h> 33 #include <utils/KeyedVector.h> 34 35 #include "MPLSensor.h" 36 37 #include "math.h" 38 #include "ml.h" 39 #include "mlFIFO.h" 40 #include "mlsl.h" 41 #include "mlos.h" 42 #include "ml_mputest.h" 43 #include "ml_stored_data.h" 44 #include "mldl_cfg.h" 45 #include "mldl.h" 46 47 #include "mpu.h" 48 #include "accel.h" 49 #include "compass.h" 50 #include "kernel/timerirq.h" 51 #include "kernel/mpuirq.h" 52 #include "kernel/slaveirq.h" 53 54 extern "C" { 55 #include "mlsupervisor.h" 56 } 57 58 #include "mlcontrol.h" 59 #include "sensor_params.h" 60 61 #define EXTRA_VERBOSE (0) 62 //#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__) 63 #define FUNC_LOG 64 #define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) 65 /* this mask must turn on only the sensors that are present and managed by the MPL */ 66 #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) 67 68 #define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) 69 70 /******************************************/ 71 72 /* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */ 73 static struct sensor_t sSensorList[] = 74 { { "MPL Gyroscope", "Invensense", 1, 75 SENSORS_GYROSCOPE_HANDLE, 76 SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, 77 { "MPL Accelerometer", "Invensense", 1, 78 SENSORS_ACCELERATION_HANDLE, 79 SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, 80 { "MPL Magnetic Field", "Invensense", 1, 81 SENSORS_MAGNETIC_FIELD_HANDLE, 82 SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, 83 { "MPL Orientation", "Invensense", 1, 84 SENSORS_ORIENTATION_HANDLE, 85 SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, { } }, 86 { "MPL Rotation Vector", "Invensense", 1, 87 SENSORS_ROTATION_VECTOR_HANDLE, 88 SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, 89 { "MPL Linear Acceleration", "Invensense", 1, 90 SENSORS_LINEAR_ACCEL_HANDLE, 91 SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, 92 { "MPL Gravity", "Invensense", 1, 93 SENSORS_GRAVITY_HANDLE, 94 SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, 95 }; 96 97 static unsigned long long irq_timestamp = 0; 98 /* *************************************************************************** 99 * MPL interface misc. 100 */ 101 //static pointer to the object that will handle callbacks 102 static MPLSensor* gMPLSensor = NULL; 103 104 /* we need to pass some callbacks to the MPL. The mpl is a C library, so 105 * wrappers for the C++ callback implementations are required. 106 */ 107 extern "C" { 108 //callback wrappers go here 109 void mot_cb_wrapper(uint16_t val) 110 { 111 if (gMPLSensor) { 112 gMPLSensor->cbOnMotion(val); 113 } 114 } 115 116 void procData_cb_wrapper() 117 { 118 if(gMPLSensor) { 119 gMPLSensor->cbProcData(); 120 } 121 } 122 123 } //end of extern C 124 125 void setCallbackObject(MPLSensor* gbpt) 126 { 127 gMPLSensor = gbpt; 128 } 129 130 131 /***************************************************************************** 132 * sensor class implementation 133 */ 134 135 #define GY_ENABLED ((1<<ID_GY) & enabled_sensors) 136 #define A_ENABLED ((1<<ID_A) & enabled_sensors) 137 #define O_ENABLED ((1<<ID_O) & enabled_sensors) 138 #define M_ENABLED ((1<<ID_M) & enabled_sensors) 139 #define LA_ENABLED ((1<<ID_LA) & enabled_sensors) 140 #define GR_ENABLED ((1<<ID_GR) & enabled_sensors) 141 #define RV_ENABLED ((1<<ID_RV) & enabled_sensors) 142 143 MPLSensor::MPLSensor() : 144 SensorBase(NULL, NULL), 145 mNewData(0), 146 mDmpStarted(false), 147 mMasterSensorMask(INV_ALL_SENSORS), 148 mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1), 149 mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false), 150 mUseTimerIrqAccel(false), mUsetimerIrqCompass(true), 151 mUseTimerirq(false), mSampleCount(0), 152 mEnabled(0), mPendingMask(0) 153 { 154 FUNC_LOG; 155 inv_error_t rv; 156 int mpu_int_fd, i; 157 char *port = NULL; 158 159 ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors); 160 161 pthread_mutex_init(&mMplMutex, NULL); 162 163 mForceSleep = false; 164 165 /* used for identifying whether 9axis is enabled or not */ 166 /* this variable will be changed in initMPL() when libmpl is loaded */ 167 /* sensor list will be changed based on this variable */ 168 mNineAxisEnabled = false; 169 170 for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 171 mPollFds[i].fd = -1; 172 mPollFds[i].events = 0; 173 } 174 175 pthread_mutex_lock(&mMplMutex); 176 177 mpu_int_fd = open("/dev/mpuirq", O_RDWR); 178 if (mpu_int_fd == -1) { 179 ALOGE("could not open the mpu irq device node"); 180 } else { 181 fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK); 182 //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0); 183 mIrqFds.add(MPUIRQ_FD, mpu_int_fd); 184 mPollFds[MPUIRQ_FD].fd = mpu_int_fd; 185 mPollFds[MPUIRQ_FD].events = POLLIN; 186 } 187 188 accel_fd = open("/dev/accelirq", O_RDWR); 189 if (accel_fd == -1) { 190 ALOGE("could not open the accel irq device node"); 191 } else { 192 fcntl(accel_fd, F_SETFL, O_NONBLOCK); 193 //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0); 194 mIrqFds.add(ACCELIRQ_FD, accel_fd); 195 mPollFds[ACCELIRQ_FD].fd = accel_fd; 196 mPollFds[ACCELIRQ_FD].events = POLLIN; 197 } 198 199 timer_fd = open("/dev/timerirq", O_RDWR); 200 if (timer_fd == -1) { 201 ALOGE("could not open the timer irq device node"); 202 } else { 203 fcntl(timer_fd, F_SETFL, O_NONBLOCK); 204 //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0); 205 mIrqFds.add(TIMERIRQ_FD, timer_fd); 206 mPollFds[TIMERIRQ_FD].fd = timer_fd; 207 mPollFds[TIMERIRQ_FD].events = POLLIN; 208 } 209 210 data_fd = mpu_int_fd; 211 212 if ((accel_fd == -1) && (timer_fd != -1)) { 213 //no accel irq and timer available 214 mUseTimerIrqAccel = true; 215 //ALOGD("MPLSensor falling back to timerirq for accel data"); 216 } 217 218 memset(mPendingEvents, 0, sizeof(mPendingEvents)); 219 220 mPendingEvents[RotationVector].version = sizeof(sensors_event_t); 221 mPendingEvents[RotationVector].sensor = ID_RV; 222 mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; 223 224 mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); 225 mPendingEvents[LinearAccel].sensor = ID_LA; 226 mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; 227 228 mPendingEvents[Gravity].version = sizeof(sensors_event_t); 229 mPendingEvents[Gravity].sensor = ID_GR; 230 mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; 231 232 mPendingEvents[Gyro].version = sizeof(sensors_event_t); 233 mPendingEvents[Gyro].sensor = ID_GY; 234 mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; 235 236 mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); 237 mPendingEvents[Accelerometer].sensor = ID_A; 238 mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; 239 240 mPendingEvents[MagneticField].version = sizeof(sensors_event_t); 241 mPendingEvents[MagneticField].sensor = ID_M; 242 mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; 243 mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; 244 245 mPendingEvents[Orientation].version = sizeof(sensors_event_t); 246 mPendingEvents[Orientation].sensor = ID_O; 247 mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; 248 mPendingEvents[Orientation].orientation.status 249 = SENSOR_STATUS_ACCURACY_HIGH; 250 251 mHandlers[RotationVector] = &MPLSensor::rvHandler; 252 mHandlers[LinearAccel] = &MPLSensor::laHandler; 253 mHandlers[Gravity] = &MPLSensor::gravHandler; 254 mHandlers[Gyro] = &MPLSensor::gyroHandler; 255 mHandlers[Accelerometer] = &MPLSensor::accelHandler; 256 mHandlers[MagneticField] = &MPLSensor::compassHandler; 257 mHandlers[Orientation] = &MPLSensor::orienHandler; 258 259 for (int i = 0; i < numSensors; i++) 260 mDelays[i] = 30000000LLU; // 30 ms by default 261 262 if (inv_serial_start(port) != INV_SUCCESS) { 263 ALOGE("Fatal Error : could not open MPL serial interface"); 264 } 265 266 //initialize library parameters 267 initMPL(); 268 269 //setup the FIFO contents 270 setupFIFO(); 271 272 //we start the motion processing only when a sensor is enabled... 273 //rv = inv_dmp_start(); 274 //ALOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv); 275 //dmp_started = true; 276 277 pthread_mutex_unlock(&mMplMutex); 278 279 } 280 281 MPLSensor::~MPLSensor() 282 { 283 FUNC_LOG; 284 pthread_mutex_lock(&mMplMutex); 285 if (inv_dmp_stop() != INV_SUCCESS) { 286 ALOGW("Error: could not stop the DMP correctly.\n"); 287 } 288 289 if (inv_dmp_close() != INV_SUCCESS) { 290 ALOGW("Error: could not close the DMP"); 291 } 292 293 if (inv_serial_stop() != INV_SUCCESS) { 294 ALOGW("Error : could not close the serial port"); 295 } 296 pthread_mutex_unlock(&mMplMutex); 297 pthread_mutex_destroy(&mMplMutex); 298 } 299 300 /* clear any data from our various filehandles */ 301 void MPLSensor::clearIrqData(bool* irq_set) 302 { 303 unsigned int i; 304 int nread; 305 struct mpuirq_data irqdata; 306 307 poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared 308 309 for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 310 int cur_fd = mPollFds[i].fd; 311 int j = 0; 312 if (mPollFds[i].revents & POLLIN) { 313 nread = read(cur_fd, &irqdata, sizeof(irqdata)); 314 if (nread > 0) { 315 irq_set[i] = true; 316 irq_timestamp = irqdata.irqtime; 317 //ALOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++); 318 } 319 } 320 mPollFds[i].revents = 0; 321 } 322 } 323 324 /* set the power states of the various sensors based on the bits set in the 325 * enabled_sensors parameter. 326 * this function modifies globalish state variables. It must be called with the mMplMutex held. */ 327 void MPLSensor::setPowerStates(int enabled_sensors) 328 { 329 FUNC_LOG; 330 bool irq_set[5] = { false, false, false, false, false }; 331 332 //ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted); 333 334 do { 335 336 if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { 337 mLocalSensorMask = ALL_MPL_SENSORS_NP; 338 break; 339 } 340 341 if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { 342 mLocalSensorMask = 0; 343 break; 344 } 345 346 if (GY_ENABLED) { 347 mLocalSensorMask |= INV_THREE_AXIS_GYRO; 348 } else { 349 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 350 } 351 352 if (A_ENABLED) { 353 mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); 354 } else { 355 mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); 356 } 357 358 if (M_ENABLED) { 359 mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 360 } else { 361 mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); 362 } 363 364 } while (0); 365 366 //record the new sensor state 367 inv_error_t rv; 368 369 long sen_mask = mLocalSensorMask & mMasterSensorMask; 370 371 bool changing_sensors = ((inv_get_dl_config()->requested_sensors 372 != sen_mask) && (sen_mask != 0)); 373 bool restart = (!mDmpStarted) && (sen_mask != 0); 374 375 if (changing_sensors || restart) { 376 377 ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); 378 379 if (mDmpStarted) { 380 inv_dmp_stop(); 381 clearIrqData(irq_set); 382 mDmpStarted = false; 383 } 384 385 if (sen_mask != inv_get_dl_config()->requested_sensors) { 386 //ALOGV("setPowerStates: %lx", sen_mask); 387 rv = inv_set_mpu_sensors(sen_mask); 388 ALOGE_IF(rv != INV_SUCCESS, 389 "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", 390 sen_mask, rv); 391 } 392 393 if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) 394 || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) 395 && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { 396 ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); 397 mUseTimerirq = true; 398 } else { 399 if (mUseTimerirq) { 400 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 401 clearIrqData(irq_set); 402 } 403 ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); 404 mUseTimerirq = false; 405 } 406 407 if (!mDmpStarted) { 408 if (mHaveGoodMpuCal || mHaveGoodCompassCal) { 409 rv = inv_store_calibration(); 410 ALOGE_IF(rv != INV_SUCCESS, 411 "error: unable to store MPL calibration file"); 412 mHaveGoodMpuCal = false; 413 mHaveGoodCompassCal = false; 414 } 415 //ALOGV("Starting DMP"); 416 rv = inv_dmp_start(); 417 ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); 418 mDmpStarted = true; 419 } 420 } 421 422 //check if we should stop the DMP 423 if (mDmpStarted && (sen_mask == 0)) { 424 //ALOGV("Stopping DMP"); 425 rv = inv_dmp_stop(); 426 ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", 427 rv); 428 if (mUseTimerirq) { 429 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 430 } 431 clearIrqData(irq_set); 432 433 mDmpStarted = false; 434 mPollTime = -1; 435 mCurFifoRate = -1; 436 } 437 438 } 439 440 /** 441 * container function for all the calls we make once to set up the MPL. 442 */ 443 void MPLSensor::initMPL() 444 { 445 FUNC_LOG; 446 inv_error_t result; 447 unsigned short bias_update_mask = 0xFFFF; 448 struct mldl_cfg *mldl_cfg; 449 450 if (inv_dmp_open() != INV_SUCCESS) { 451 ALOGE("Fatal Error : could not open DMP correctly.\n"); 452 } 453 454 result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work 455 ALOGE_IF(result != INV_SUCCESS, 456 "Fatal Error : could not set enabled sensors."); 457 458 if (inv_load_calibration() != INV_SUCCESS) { 459 ALOGE("could not open MPL calibration file"); 460 } 461 462 //check for the 9axis fusion library: if available load it and start 9x 463 void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); 464 if(h_dmp_lib) { 465 const char* error; 466 error = dlerror(); 467 inv_error_t (*fp_inv_enable_9x_fusion)() = 468 (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); 469 if((error = dlerror()) != NULL) { 470 ALOGE("%s %s", error, "inv_enable_9x_fusion"); 471 } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { 472 ALOGE( "Warning : 9 axis sensor fusion not available " 473 "- No compass detected.\n"); 474 } else { 475 /* 9axis is loaded and enabled */ 476 /* this variable is used for coming up with sensor list */ 477 mNineAxisEnabled = true; 478 } 479 } else { 480 const char* error = dlerror(); 481 ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); 482 } 483 484 mldl_cfg = inv_get_dl_config(); 485 486 if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { 487 ALOGE("Error : Bias update function could not be set.\n"); 488 } 489 490 if (inv_set_motion_interrupt(1) != INV_SUCCESS) { 491 ALOGE("Error : could not set motion interrupt"); 492 } 493 494 if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { 495 ALOGE("Error : could not set fifo interrupt"); 496 } 497 498 result = inv_set_fifo_rate(6); 499 if (result != INV_SUCCESS) { 500 ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); 501 } 502 503 setupCallbacks(); 504 505 } 506 507 /** setup the fifo contents. 508 */ 509 void MPLSensor::setupFIFO() 510 { 511 FUNC_LOG; 512 inv_error_t result; 513 514 result = inv_send_accel(INV_ALL, INV_32_BIT); 515 if (result != INV_SUCCESS) { 516 ALOGE("Fatal error: inv_send_accel returned %d\n", result); 517 } 518 519 result = inv_send_quaternion(INV_32_BIT); 520 if (result != INV_SUCCESS) { 521 ALOGE("Fatal error: inv_send_quaternion returned %d\n", result); 522 } 523 524 result = inv_send_linear_accel(INV_ALL, INV_32_BIT); 525 if (result != INV_SUCCESS) { 526 ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result); 527 } 528 529 result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); 530 if (result != INV_SUCCESS) { 531 ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", 532 result); 533 } 534 535 result = inv_send_gravity(INV_ALL, INV_32_BIT); 536 if (result != INV_SUCCESS) { 537 ALOGE("Fatal error: inv_send_gravity returned %d\n", result); 538 } 539 540 result = inv_send_gyro(INV_ALL, INV_32_BIT); 541 if (result != INV_SUCCESS) { 542 ALOGE("Fatal error: inv_send_gyro returned %d\n", result); 543 } 544 545 } 546 547 /** 548 * set up the callbacks that we use in all cases (outside of gestures, etc) 549 */ 550 void MPLSensor::setupCallbacks() 551 { 552 FUNC_LOG; 553 if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { 554 ALOGE("Error : Motion callback could not be set.\n"); 555 556 } 557 558 if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { 559 ALOGE("Error : Processed data callback could not be set."); 560 561 } 562 } 563 564 /** 565 * handle the motion/no motion output from the MPL. 566 */ 567 void MPLSensor::cbOnMotion(uint16_t val) 568 { 569 FUNC_LOG; 570 //after the first no motion, the gyro should be calibrated well 571 if (val == 2) { 572 if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { 573 //if gyros are on and we got a no motion, set a flag 574 // indicating that the cal file can be written. 575 mHaveGoodMpuCal = true; 576 } 577 } 578 579 return; 580 } 581 582 583 void MPLSensor::cbProcData() 584 { 585 mNewData = 1; 586 mSampleCount++; 587 //ALOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount); 588 } 589 590 //these handlers transform mpl data into one of the Android sensor types 591 // scaling and coordinate transforms should be done in the handlers 592 593 void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, 594 int index) 595 { 596 VFUNC_LOG; 597 inv_error_t res; 598 res = inv_get_float_array(INV_GYROS, s->gyro.v); 599 s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; 600 s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; 601 s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; 602 if (res == INV_SUCCESS) 603 *pending_mask |= (1 << index); 604 } 605 606 void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, 607 int index) 608 { 609 //VFUNC_LOG; 610 inv_error_t res; 611 res = inv_get_float_array(INV_ACCELS, s->acceleration.v); 612 //res = inv_get_accel_float(s->acceleration.v); 613 s->acceleration.v[0] = s->acceleration.v[0] * 9.81; 614 s->acceleration.v[1] = s->acceleration.v[1] * 9.81; 615 s->acceleration.v[2] = s->acceleration.v[2] * 9.81; 616 //ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]); 617 if (res == INV_SUCCESS) 618 *pending_mask |= (1 << index); 619 } 620 621 int MPLSensor::estimateCompassAccuracy() 622 { 623 inv_error_t res; 624 int rv; 625 626 res = inv_get_compass_accuracy(&rv); 627 if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) { 628 mHaveGoodCompassCal = true; 629 } 630 ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); 631 632 return rv; 633 } 634 635 void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, 636 int index) 637 { 638 VFUNC_LOG; 639 inv_error_t res, res2; 640 float bias_error[3]; 641 float total_be; 642 static int bias_error_settled = 0; 643 644 res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); 645 646 if (res != INV_SUCCESS) { 647 ALOGW( 648 "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", 649 res); 650 } 651 652 s->magnetic.status = estimateCompassAccuracy(); 653 654 if (res == INV_SUCCESS) 655 *pending_mask |= (1 << index); 656 } 657 658 void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, 659 int index) 660 { 661 VFUNC_LOG; 662 float quat[4]; 663 float norm = 0; 664 float ang = 0; 665 inv_error_t r; 666 667 r = inv_get_float_array(INV_QUATERNION, quat); 668 669 if (r != INV_SUCCESS) { 670 *pending_mask &= ~(1 << index); 671 return; 672 } else { 673 *pending_mask |= (1 << index); 674 } 675 676 norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] 677 + FLT_EPSILON; 678 679 if (norm > 1.0f) { 680 //renormalize 681 norm = sqrtf(norm); 682 float inv_norm = 1.0f / norm; 683 quat[1] = quat[1] * inv_norm; 684 quat[2] = quat[2] * inv_norm; 685 quat[3] = quat[3] * inv_norm; 686 } 687 688 if (quat[0] < 0.0) { 689 quat[1] = -quat[1]; 690 quat[2] = -quat[2]; 691 quat[3] = -quat[3]; 692 } 693 694 s->gyro.v[0] = quat[1]; 695 s->gyro.v[1] = quat[2]; 696 s->gyro.v[2] = quat[3]; 697 698 } 699 700 void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, 701 int index) 702 { 703 VFUNC_LOG; 704 inv_error_t res; 705 res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); 706 s->gyro.v[0] *= 9.81; 707 s->gyro.v[1] *= 9.81; 708 s->gyro.v[2] *= 9.81; 709 if (res == INV_SUCCESS) 710 *pending_mask |= (1 << index); 711 } 712 713 void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, 714 int index) 715 { 716 VFUNC_LOG; 717 inv_error_t res; 718 res = inv_get_float_array(INV_GRAVITY, s->gyro.v); 719 s->gyro.v[0] *= 9.81; 720 s->gyro.v[1] *= 9.81; 721 s->gyro.v[2] *= 9.81; 722 if (res == INV_SUCCESS) 723 *pending_mask |= (1 << index); 724 } 725 726 void MPLSensor::calcOrientationSensor(float *R, float *values) 727 { 728 float tmp; 729 730 //Azimuth 731 if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { 732 values[0] = (float) atan2f(-R[3], R[0]); 733 } else { 734 values[0] = (float) atan2f(R[1], R[4]); 735 } 736 values[0] *= 57.295779513082320876798154814105f; 737 if (values[0] < 0) { 738 values[0] += 360.0f; 739 } 740 //Pitch 741 tmp = R[7]; 742 if (tmp > 1.0f) 743 tmp = 1.0f; 744 if (tmp < -1.0f) 745 tmp = -1.0f; 746 values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; 747 if (R[8] < 0) { 748 values[1] = 180.0f - values[1]; 749 } 750 if (values[1] > 180.0f) { 751 values[1] -= 360.0f; 752 } 753 //Roll 754 if ((R[7] > 0.7071067f)) { 755 values[2] = (float) atan2f(R[6], R[7]); 756 } else { 757 values[2] = (float) atan2f(R[6], R[8]); 758 } 759 760 values[2] *= 57.295779513082320876798154814105f; 761 if (values[2] > 90.0f) { 762 values[2] = 180.0f - values[2]; 763 } 764 if (values[2] < -90.0f) { 765 values[2] = -180.0f - values[2]; 766 } 767 } 768 769 void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, 770 int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output 771 { 772 VFUNC_LOG; 773 inv_error_t res; 774 float euler[3]; 775 float heading[1]; 776 float rot_mat[9]; 777 778 res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); 779 780 //ComputeAndOrientation(heading[0], euler, s->orientation.v); 781 calcOrientationSensor(rot_mat, s->orientation.v); 782 783 s->orientation.status = estimateCompassAccuracy(); 784 785 if (res == INV_SUCCESS) 786 *pending_mask |= (1 << index); 787 else 788 ALOGW("orienHandler: data not valid (%d)", (int) res); 789 790 } 791 792 int MPLSensor::enable(int32_t handle, int en) 793 { 794 FUNC_LOG; 795 //ALOGV("handle : %d en: %d", handle, en); 796 797 int what = -1; 798 799 switch (handle) { 800 case ID_A: 801 what = Accelerometer; 802 break; 803 case ID_M: 804 what = MagneticField; 805 break; 806 case ID_O: 807 what = Orientation; 808 break; 809 case ID_GY: 810 what = Gyro; 811 break; 812 case ID_GR: 813 what = Gravity; 814 break; 815 case ID_RV: 816 what = RotationVector; 817 break; 818 case ID_LA: 819 what = LinearAccel; 820 break; 821 default: //this takes care of all the gestures 822 what = handle; 823 break; 824 } 825 826 if (uint32_t(what) >= numSensors) 827 return -EINVAL; 828 829 int newState = en ? 1 : 0; 830 int err = 0; 831 //ALOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)), 832 // "sensor state change what=%d", what); 833 834 pthread_mutex_lock(&mMplMutex); 835 if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { 836 uint32_t sensor_type; 837 short flags = newState; 838 mEnabled &= ~(1 << what); 839 mEnabled |= (uint32_t(flags) << what); 840 ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); 841 setPowerStates(mEnabled); 842 pthread_mutex_unlock(&mMplMutex); 843 if (!newState) 844 update_delay(); 845 return err; 846 } 847 pthread_mutex_unlock(&mMplMutex); 848 return err; 849 } 850 851 int MPLSensor::setDelay(int32_t handle, int64_t ns) 852 { 853 FUNC_LOG; 854 ALOGV_IF(EXTRA_VERBOSE, 855 " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); 856 int what = -1; 857 switch (handle) { 858 case ID_A: 859 what = Accelerometer; 860 break; 861 case ID_M: 862 what = MagneticField; 863 break; 864 case ID_O: 865 what = Orientation; 866 break; 867 case ID_GY: 868 what = Gyro; 869 break; 870 case ID_GR: 871 what = Gravity; 872 break; 873 case ID_RV: 874 what = RotationVector; 875 break; 876 case ID_LA: 877 what = LinearAccel; 878 break; 879 default: 880 what = handle; 881 break; 882 } 883 884 if (uint32_t(what) >= numSensors) 885 return -EINVAL; 886 887 if (ns < 0) 888 return -EINVAL; 889 890 pthread_mutex_lock(&mMplMutex); 891 mDelays[what] = ns; 892 pthread_mutex_unlock(&mMplMutex); 893 return update_delay(); 894 } 895 896 int MPLSensor::update_delay() 897 { 898 FUNC_LOG; 899 int rv = 0; 900 bool irq_set[5]; 901 902 pthread_mutex_lock(&mMplMutex); 903 904 if (mEnabled) { 905 uint64_t wanted = -1LLU; 906 for (int i = 0; i < numSensors; i++) { 907 if (mEnabled & (1 << i)) { 908 uint64_t ns = mDelays[i]; 909 wanted = wanted < ns ? wanted : ns; 910 } 911 } 912 913 //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns 914 if (wanted < 10000000LLU) { 915 wanted = 10000000LLU; 916 } 917 918 int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 919 : 0); //mpu fifo rate is in increments of 5ms 920 if (rate == 0) //KLP disallow fifo rate 0 921 rate = 1; 922 923 if (rate != mCurFifoRate) { 924 //ALOGD("set fifo rate: %d %llu", rate, wanted); 925 inv_error_t res; // = inv_dmp_stop(); 926 res = inv_set_fifo_rate(rate); 927 ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); 928 929 //res = inv_dmp_start(); 930 //ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP"); 931 932 mCurFifoRate = rate; 933 rv = (res == INV_SUCCESS); 934 } 935 936 if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { 937 if (mUseTimerirq) { 938 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 939 clearIrqData(irq_set); 940 if (inv_get_dl_config()->requested_sensors 941 == INV_THREE_AXIS_COMPASS) { 942 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 943 (unsigned long) (wanted / 1000000LLU)); 944 ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 945 (int) (wanted / 1000000LLU)); 946 } else { 947 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 948 (unsigned long) inv_get_sample_step_size_ms()); 949 ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 950 (int) inv_get_sample_step_size_ms()); 951 } 952 } 953 } 954 955 } 956 pthread_mutex_unlock(&mMplMutex); 957 return rv; 958 } 959 960 /* return the current time in nanoseconds */ 961 int64_t MPLSensor::now_ns(void) 962 { 963 //FUNC_LOG; 964 struct timespec ts; 965 966 clock_gettime(CLOCK_MONOTONIC, &ts); 967 //ALOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec); 968 return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 969 } 970 971 int MPLSensor::readEvents(sensors_event_t* data, int count) 972 { 973 //VFUNC_LOG; 974 int i; 975 bool irq_set[5] = { false, false, false, false, false }; 976 inv_error_t rv; 977 if (count < 1) 978 return -EINVAL; 979 int numEventReceived = 0; 980 981 clearIrqData(irq_set); 982 983 pthread_mutex_lock(&mMplMutex); 984 if (mDmpStarted) { 985 //ALOGV_IF(EXTRA_VERBOSE, "Update Data"); 986 rv = inv_update_data(); 987 ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); 988 } 989 990 else { 991 //probably just one extra read after shutting down 992 ALOGV_IF(EXTRA_VERBOSE, 993 "MPLSensor::readEvents called, but there's nothing to do."); 994 } 995 996 pthread_mutex_unlock(&mMplMutex); 997 998 if (!mNewData) { 999 ALOGV_IF(EXTRA_VERBOSE, "no new data"); 1000 return 0; 1001 } 1002 mNewData = 0; 1003 1004 /* google timestamp */ 1005 pthread_mutex_lock(&mMplMutex); 1006 for (int i = 0; i < numSensors; i++) { 1007 if (mEnabled & (1 << i)) { 1008 CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, 1009 &mPendingMask, i); 1010 mPendingEvents[i].timestamp = irq_timestamp; 1011 } 1012 } 1013 1014 for (int j = 0; count && mPendingMask && j < numSensors; j++) { 1015 if (mPendingMask & (1 << j)) { 1016 mPendingMask &= ~(1 << j); 1017 if (mEnabled & (1 << j)) { 1018 *data++ = mPendingEvents[j]; 1019 count--; 1020 numEventReceived++; 1021 } 1022 } 1023 1024 } 1025 1026 pthread_mutex_unlock(&mMplMutex); 1027 return numEventReceived; 1028 } 1029 1030 int MPLSensor::getFd() const 1031 { 1032 //ALOGV("MPLSensor::getFd returning %d", data_fd); 1033 return data_fd; 1034 } 1035 1036 int MPLSensor::getAccelFd() const 1037 { 1038 //ALOGV("MPLSensor::getAccelFd returning %d", accel_fd); 1039 return accel_fd; 1040 } 1041 1042 int MPLSensor::getTimerFd() const 1043 { 1044 //ALOGV("MPLSensor::getTimerFd returning %d", timer_fd); 1045 return timer_fd; 1046 } 1047 1048 int MPLSensor::getPowerFd() const 1049 { 1050 int hdl = (uintptr_t) inv_get_serial_handle(); 1051 //ALOGV("MPLSensor::getPowerFd returning %d", hdl); 1052 return hdl; 1053 } 1054 1055 int MPLSensor::getPollTime() 1056 { 1057 return mPollTime; 1058 } 1059 1060 bool MPLSensor::hasPendingEvents() const 1061 { 1062 //if we are using the polling workaround, force the main loop to check for data every time 1063 return (mPollTime != -1); 1064 } 1065 1066 void MPLSensor::handlePowerEvent() 1067 { 1068 VFUNC_LOG; 1069 mpuirq_data irqd; 1070 1071 int fd = (uintptr_t) inv_get_serial_handle(); 1072 read(fd, &irqd, sizeof(irqd)); 1073 1074 if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { 1075 //going to sleep 1076 sleepEvent(); 1077 } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { 1078 //waking up 1079 wakeEvent(); 1080 } 1081 1082 ioctl(fd, MPU_PM_EVENT_HANDLED, 0); 1083 } 1084 1085 void MPLSensor::sleepEvent() 1086 { 1087 VFUNC_LOG; 1088 pthread_mutex_lock(&mMplMutex); 1089 if (mEnabled != 0) { 1090 mForceSleep = true; 1091 mOldEnabledMask = mEnabled; 1092 setPowerStates(0); 1093 } 1094 pthread_mutex_unlock(&mMplMutex); 1095 } 1096 1097 void MPLSensor::wakeEvent() 1098 { 1099 VFUNC_LOG; 1100 pthread_mutex_lock(&mMplMutex); 1101 if (mForceSleep) { 1102 setPowerStates((mOldEnabledMask | mEnabled)); 1103 } 1104 mForceSleep = false; 1105 pthread_mutex_unlock(&mMplMutex); 1106 } 1107 1108 /** fill in the sensor list based on which sensors are configured. 1109 * return the number of configured sensors. 1110 * parameter list must point to a memory region of at least 7*sizeof(sensor_t) 1111 * parameter len gives the length of the buffer pointed to by list 1112 */ 1113 1114 int MPLSensor::populateSensorList(struct sensor_t *list, int len) 1115 { 1116 int numsensors; 1117 1118 if(len < 7*sizeof(sensor_t)) { 1119 ALOGE("sensor list too small, not populating."); 1120 return 0; 1121 } 1122 1123 /* fill in the base values */ 1124 memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); 1125 1126 /* first add gyro, accel and compass to the list */ 1127 1128 /* fill in accel values */ 1129 unsigned short accelId = inv_get_accel_id(); 1130 if(accelId == 0) 1131 { 1132 ALOGE("Can not get accel id"); 1133 } 1134 fillAccel(accelId, list); 1135 1136 /* fill in compass values */ 1137 unsigned short compassId = inv_get_compass_id(); 1138 if(compassId == 0) 1139 { 1140 ALOGE("Can not get compass id"); 1141 } 1142 fillCompass(compassId, list); 1143 1144 /* fill in gyro values */ 1145 fillGyro(MPU_NAME, list); 1146 1147 if(mNineAxisEnabled) 1148 { 1149 numsensors = 7; 1150 /* all sensors will be added to the list */ 1151 /* fill in orientation values */ 1152 fillOrientation(list); 1153 1154 /* fill in rotation vector values */ 1155 fillRV(list); 1156 1157 /* fill in gravity values */ 1158 fillGravity(list); 1159 1160 /* fill in Linear accel values */ 1161 fillLinearAccel(list); 1162 } else { 1163 /* no 9-axis sensors, zero fill that part of the list */ 1164 numsensors = 3; 1165 memset(list+3, 0, 4*sizeof(struct sensor_t)); 1166 } 1167 1168 return numsensors; 1169 } 1170 1171 void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list) 1172 { 1173 switch (accel) { 1174 case ACCEL_ID_LIS331: 1175 list[Accelerometer].maxRange = ACCEL_LIS331_RANGE; 1176 list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION; 1177 list[Accelerometer].power = ACCEL_LIS331_POWER; 1178 break; 1179 1180 case ACCEL_ID_LIS3DH: 1181 list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE; 1182 list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION; 1183 list[Accelerometer].power = ACCEL_LIS3DH_POWER; 1184 break; 1185 1186 case ACCEL_ID_KXSD9: 1187 list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE; 1188 list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION; 1189 list[Accelerometer].power = ACCEL_KXSD9_POWER; 1190 break; 1191 1192 case ACCEL_ID_KXTF9: 1193 list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE; 1194 list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION; 1195 list[Accelerometer].power = ACCEL_KXTF9_POWER; 1196 break; 1197 1198 case ACCEL_ID_BMA150: 1199 list[Accelerometer].maxRange = ACCEL_BMA150_RANGE; 1200 list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION; 1201 list[Accelerometer].power = ACCEL_BMA150_POWER; 1202 break; 1203 1204 case ACCEL_ID_BMA222: 1205 list[Accelerometer].maxRange = ACCEL_BMA222_RANGE; 1206 list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION; 1207 list[Accelerometer].power = ACCEL_BMA222_POWER; 1208 break; 1209 1210 case ACCEL_ID_BMA250: 1211 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 1212 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 1213 list[Accelerometer].power = ACCEL_BMA250_POWER; 1214 break; 1215 1216 case ACCEL_ID_ADXL34X: 1217 list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE; 1218 list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION; 1219 list[Accelerometer].power = ACCEL_ADXL34X_POWER; 1220 break; 1221 1222 case ACCEL_ID_MMA8450: 1223 list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1224 list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1225 list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1226 break; 1227 1228 case ACCEL_ID_MMA845X: 1229 list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE; 1230 list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION; 1231 list[Accelerometer].power = ACCEL_MMA845X_POWER; 1232 break; 1233 1234 case ACCEL_ID_MPU6050: 1235 list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; 1236 list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; 1237 list[Accelerometer].power = ACCEL_MPU6050_POWER; 1238 break; 1239 default: 1240 ALOGE("unknown accel id -- accel params will be wrong."); 1241 break; 1242 } 1243 } 1244 1245 void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list) 1246 { 1247 switch (compass) { 1248 case COMPASS_ID_AK8975: 1249 list[MagneticField].maxRange = COMPASS_AKM8975_RANGE; 1250 list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION; 1251 list[MagneticField].power = COMPASS_AKM8975_POWER; 1252 break; 1253 case COMPASS_ID_AMI30X: 1254 list[MagneticField].maxRange = COMPASS_AMI30X_RANGE; 1255 list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION; 1256 list[MagneticField].power = COMPASS_AMI30X_POWER; 1257 break; 1258 case COMPASS_ID_AMI306: 1259 list[MagneticField].maxRange = COMPASS_AMI306_RANGE; 1260 list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1261 list[MagneticField].power = COMPASS_AMI306_POWER; 1262 break; 1263 case COMPASS_ID_YAS529: 1264 list[MagneticField].maxRange = COMPASS_YAS529_RANGE; 1265 list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1266 list[MagneticField].power = COMPASS_AMI306_POWER; 1267 break; 1268 case COMPASS_ID_YAS530: 1269 list[MagneticField].maxRange = COMPASS_YAS530_RANGE; 1270 list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; 1271 list[MagneticField].power = COMPASS_YAS530_POWER; 1272 break; 1273 case COMPASS_ID_HMC5883: 1274 list[MagneticField].maxRange = COMPASS_HMC5883_RANGE; 1275 list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION; 1276 list[MagneticField].power = COMPASS_HMC5883_POWER; 1277 break; 1278 case COMPASS_ID_MMC314X: 1279 list[MagneticField].maxRange = COMPASS_MMC314X_RANGE; 1280 list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION; 1281 list[MagneticField].power = COMPASS_MMC314X_POWER; 1282 break; 1283 case COMPASS_ID_HSCDTD002B: 1284 list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE; 1285 list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION; 1286 list[MagneticField].power = COMPASS_HSCDTD002B_POWER; 1287 break; 1288 case COMPASS_ID_HSCDTD004A: 1289 list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE; 1290 list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION; 1291 list[MagneticField].power = COMPASS_HSCDTD004A_POWER; 1292 break; 1293 default: 1294 ALOGE("unknown compass id -- compass parameters will be wrong"); 1295 } 1296 } 1297 1298 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 1299 { 1300 if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) { 1301 list[Gyro].maxRange = GYRO_MPU3050_RANGE; 1302 list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 1303 list[Gyro].power = GYRO_MPU3050_POWER; 1304 } else { 1305 list[Gyro].maxRange = GYRO_MPU6050_RANGE; 1306 list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; 1307 list[Gyro].power = GYRO_MPU6050_POWER; 1308 } 1309 return; 1310 } 1311 1312 1313 /* fillRV depends on values of accel and compass in the list */ 1314 void MPLSensor::fillRV(struct sensor_t *list) 1315 { 1316 /* compute power on the fly */ 1317 list[RotationVector].power = list[Gyro].power + list[Accelerometer].power 1318 + list[MagneticField].power; 1319 list[RotationVector].resolution = .00001; 1320 list[RotationVector].maxRange = 1.0; 1321 return; 1322 } 1323 1324 void MPLSensor::fillOrientation(struct sensor_t *list) 1325 { 1326 list[Orientation].power = list[Gyro].power + list[Accelerometer].power 1327 + list[MagneticField].power; 1328 list[Orientation].resolution = .00001; 1329 list[Orientation].maxRange = 360.0; 1330 return; 1331 } 1332 1333 void MPLSensor::fillGravity( struct sensor_t *list) 1334 { 1335 list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1336 + list[MagneticField].power; 1337 list[Gravity].resolution = .00001; 1338 list[Gravity].maxRange = 9.81; 1339 return; 1340 } 1341 1342 void MPLSensor::fillLinearAccel(struct sensor_t *list) 1343 { 1344 list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1345 + list[MagneticField].power; 1346 list[Gravity].resolution = list[Accelerometer].resolution; 1347 list[Gravity].maxRange = list[Accelerometer].maxRange; 1348 return; 1349 } 1350
