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