/**
* @brief umplInit is the entry point to uMPL and must be called before
* all other functions. It initializes the platform specific data.
* It also initializes the MPL state.
*
*
* @param data
* A dummy port number. Could be NULL.
*
*/
inv_error_t umplInit(unsigned char *port)
{
if (umplState == UMPL_UNINIT)
{
inv_error_t result;
result = inv_ustore_enable();
if (result != INV_SUCCESS) return result;
result = inv_init_ustore_lite_fusion();
if (result != INV_SUCCESS) return result;
result = umplPlatformSetup();
if (result != INV_SUCCESS) return result;
result = inv_serial_start((char const *)port);
if (result != INV_SUCCESS) return result;
umplSetState(UMPL_STOP);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
MPLSensor::MPLSensor() :
SensorBase(NULL, NULL),
mNewData(0),
mDmpStarted(false),
mMasterSensorMask(INV_ALL_SENSORS),
mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
mUseTimerirq(false), mSampleCount(0),
mEnabled(0), mPendingMask(0)
{
FUNC_LOG;
inv_error_t rv;
int mpu_int_fd, i;
char *port = NULL;
ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors);
pthread_mutex_init(&mMplMutex, NULL);
mForceSleep = false;
/* used for identifying whether 9axis is enabled or not */
/* this variable will be changed in initMPL() when libmpl is loaded */
/* sensor list will be changed based on this variable */
mNineAxisEnabled = false;
for (i = 0; i < ARRAY_SIZE(mPollFds); i++) {
mPollFds[i].fd = -1;
mPollFds[i].events = 0;
}
pthread_mutex_lock(&mMplMutex);
mpu_int_fd = open("/dev/mpuirq", O_RDWR);
if (mpu_int_fd == -1) {
ALOGE("could not open the mpu irq device node");
} else {
fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK);
//ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0);
mIrqFds.add(MPUIRQ_FD, mpu_int_fd);
mPollFds[MPUIRQ_FD].fd = mpu_int_fd;
mPollFds[MPUIRQ_FD].events = POLLIN;
}
accel_fd = open("/dev/accelirq", O_RDWR);
if (accel_fd == -1) {
ALOGE("could not open the accel irq device node");
} else {
fcntl(accel_fd, F_SETFL, O_NONBLOCK);
//ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0);
mIrqFds.add(ACCELIRQ_FD, accel_fd);
mPollFds[ACCELIRQ_FD].fd = accel_fd;
mPollFds[ACCELIRQ_FD].events = POLLIN;
}
timer_fd = open("/dev/timerirq", O_RDWR);
if (timer_fd == -1) {
ALOGE("could not open the timer irq device node");
} else {
fcntl(timer_fd, F_SETFL, O_NONBLOCK);
//ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0);
mIrqFds.add(TIMERIRQ_FD, timer_fd);
mPollFds[TIMERIRQ_FD].fd = timer_fd;
mPollFds[TIMERIRQ_FD].events = POLLIN;
}
data_fd = mpu_int_fd;
if ((accel_fd == -1) && (timer_fd != -1)) {
//no accel irq and timer available
mUseTimerIrqAccel = true;
//ALOGD("MPLSensor falling back to timerirq for accel data");
}
memset(mPendingEvents, 0, sizeof(mPendingEvents));
mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
mPendingEvents[RotationVector].sensor = ID_RV;
mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
mPendingEvents[LinearAccel].sensor = ID_LA;
mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
mPendingEvents[Gravity].version = sizeof(sensors_event_t);
mPendingEvents[Gravity].sensor = ID_GR;
mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
mPendingEvents[Gyro].version = sizeof(sensors_event_t);
mPendingEvents[Gyro].sensor = ID_GY;
mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
mPendingEvents[Accelerometer].sensor = ID_A;
mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
mPendingEvents[MagneticField].sensor = ID_M;
mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
mPendingEvents[Orientation].version = sizeof(sensors_event_t);
mPendingEvents[Orientation].sensor = ID_O;
mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
mPendingEvents[Orientation].orientation.status
= SENSOR_STATUS_ACCURACY_HIGH;
mHandlers[RotationVector] = &MPLSensor::rvHandler;
mHandlers[LinearAccel] = &MPLSensor::laHandler;
mHandlers[Gravity] = &MPLSensor::gravHandler;
mHandlers[Gyro] = &MPLSensor::gyroHandler;
mHandlers[Accelerometer] = &MPLSensor::accelHandler;
mHandlers[MagneticField] = &MPLSensor::compassHandler;
mHandlers[Orientation] = &MPLSensor::orienHandler;
for (int i = 0; i < numSensors; i++)
mDelays[i] = 30000000LLU; // 30 ms by default
if (inv_serial_start(port) != INV_SUCCESS) {
ALOGE("Fatal Error : could not open MPL serial interface");
}
//initialize library parameters
initMPL();
//setup the FIFO contents
setupFIFO();
//we start the motion processing only when a sensor is enabled...
//rv = inv_dmp_start();
//ALOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv);
//dmp_started = true;
pthread_mutex_unlock(&mMplMutex);
}
// Main function
int main(int argc, char *argv[])
{
//unsigned short accelSlaveAddr = ACCEL_SLAVEADDR_INVALID;
unsigned short platformId = ID_INVALID;
unsigned short accelId = ID_INVALID;
unsigned short compassId = ID_INVALID;
unsigned char reg[32];
unsigned char *verStr;
int result;
int key = 0;
int interror;
struct mpuirq_data **data;
const char *ints[] = { "/dev/mpuirq", /* INTSRC_MPU */
//"/dev/accelirq", /* INTSRC_AUX1 */
//"/dev/compassirq", /* INTSRC_AUX2 */
//"/dev/pressureirq", /* INTSRC_AUX3 */
};
int handles[ARRAY_SIZE(ints)];
CALL_N_CHECK( inv_get_version(&verStr) );
printf("%s\n", verStr);
if(INV_SUCCESS == MenuHwChoice(&platformId, &accelId, &compassId)) {
CALL_CHECK_N_RETURN_ERROR(SetupPlatform(platformId,
accelId, compassId));
}
CALL_CHECK_N_RETURN_ERROR( inv_serial_start("/dev/mpu") );
IntOpen(ints, handles, ARRAY_SIZE(ints));
if (handles[0] < 0) {
printf("IntOpen failed\n");
interror = INV_ERROR;
} else {
interror = INV_SUCCESS;
}
CALL_CHECK_N_RETURN_ERROR( inv_dmp_open() );
CALL_CHECK_N_RETURN_ERROR(inv_set_mpu_sensors(INV_NINE_AXIS));
/***********************/
/* advanced algorithms */
/***********************/
/* The Aichi and AKM libraries are only built against the
* android tool chain */
CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_no_motion());
CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_from_gravity(true));
CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_from_LPF(true));
CALL_CHECK_N_RETURN_ERROR(inv_set_dead_zone_normal(true));
#ifdef ANDROID
{
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id == COMPASS_ID_AK8975) {
CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external());
CALL_CHECK_N_RETURN_ERROR(inv_external_slave_akm8975_open());
} else if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id ==
COMPASS_ID_AMI306) {
CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external());
CALL_CHECK_N_RETURN_ERROR(inv_external_slave_ami306_open());
} else if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id ==
COMPASS_ID_MMC328X) {
printf("Compass id is %d\n", mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id);
CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external());
CALL_CHECK_N_RETURN_ERROR(inv_external_slave_mmc3280_open());
}else {
CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion());
}
}
#else
CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion());
#endif
CALL_CHECK_N_RETURN_ERROR( inv_enable_temp_comp() );
CALL_CHECK_N_RETURN_ERROR( inv_enable_fast_nomot() );
CALL_CHECK_N_RETURN_ERROR( inv_set_motion_callback(onMotion) );
CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_processed_callback(processedData) );
/* Setup FIFO */
CALL_CHECK_N_RETURN_ERROR( inv_send_quaternion(INV_32_BIT) );
CALL_CHECK_N_RETURN_ERROR( inv_send_gyro(INV_ALL,INV_32_BIT) );
CALL_CHECK_N_RETURN_ERROR( inv_send_accel(INV_ALL,INV_32_BIT) );
CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_rate(20) );
/* Check to see if interrupts are available. If so use them */
if (INV_SUCCESS == interror) {
CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_interrupt(true) );
CALL_CHECK_N_RETURN_ERROR( inv_set_motion_interrupt(true) );
CALL_CHECK_N_RETURN_ERROR( IntSetTimeout(handles[0], 100) );
MPL_LOGI("Interrupts Configured\n");
flag |= 0x04;
} else {
MPL_LOGI("Interrupts unavailable on this platform\n");
flag &= ~0x04;
}
CALL_CHECK_N_RETURN_ERROR( inv_dmp_start() );
//Loop
while (1) {
usleep(8000);
result = ConsoleKbhit();
if (DEBUG_OUT)
printf("_kbhit result : %d\n", result);
if (result) {
key = ConsoleGetChar();
if (DEBUG_OUT)
printf("getchar key : %c (%d)\n", key, key);
} else{
key = 0;
}
if (key == 'q') {
printf("quit...\n");
break;
} else if (key == '0') {
printf("flag=0\n");
flag = 0;
} else if (key == '1') {
if (flag & 1) {
MPL_LOGI("flag &= ~1 - who am i\n");
flag &= ~1;
} else {
MPL_LOGI("flag |= 1 - who am i\n");
flag |= 1;
}
} else if (key == '2') {
if (flag & 2) {
MPL_LOGI("flag &= ~2 - inv_update_data()\n");
flag &= ~2;
} else {
MPL_LOGI("flag |= 2 - inv_update_data()\n");
flag |= 2;
}
} else if (key == '4') {
if (flag & 4) {
MPL_LOGI("flag &= ~4 - IntProcess()\n");
flag &= ~4;
} else {
MPL_LOGI("flag |= 4 - IntProcess()\n");
flag |= 4;
}
} else if (key == 'a') {
if (flag & 0x80) {
MPL_LOGI("flag &= ~0x80 - Quaternion\n");
flag &= ~0x80;
} else {
MPL_LOGI("flag |= 0x80 - Quaternion\n");
flag |= 0x80;
}
} else if (key == 'b') {
if (flag & 0x20) {
printf("flag &= ~0x20 - dumpData()\n");
flag &= ~0x20;
} else {
printf("flag |= 0x20 - dumpData()\n");
flag |= 0x20;
}
} else if (key == 'S') {
MPL_LOGI("run MPU Self-Test...\n");
CALL_CHECK_N_RETURN_ERROR(inv_self_test_run());
inv_sleep(5);
continue;
} else if (key == 'C') {
MPL_LOGI("run MPU Calibration Test...\n");
CALL_CHECK_N_RETURN_ERROR(inv_self_test_calibration_run());
inv_sleep(5);
continue;
} else if (key == 'Z') {
MPL_LOGI("run MPU Self-Test for Accel Z-Axis...\n");
CALL_CHECK_N_RETURN_ERROR(inv_self_test_accel_z_run());
inv_sleep(5);
continue;
} else if (key == 'h') {
printf(
"\n\n"
"0 - turn all the features OFF\n"
"1 - read WHO_AM_I\n"
"2 - call inv_update_data()\n"
"4 - call IntProcess()\n"
"a - print Quaternion data\n"
"b - Print raw accel and gyro data\n"
"S - interrupt execution, run self-test\n"
"C - interrupt execution, run calibration test\n"
"Z - interrupt execution, run accel Z-axis test\n"
"h - show this help\n"
"\n\n"
);
}
if (flag & 0x01) {
if (DEBUG_OUT)
printf("inv_serial_readSingle(0x68,0,reg)\n");
CALL_CHECK_N_RETURN_ERROR(
inv_serial_read(inv_get_serial_handle(), 0x68, 0, 1, reg) );
printf("\nreg[0]=%02x", reg[0]);
}
if (flag & 0x02) {
CALL_N_CHECK( inv_update_data() );
}
if (flag & 0x04) {
data = InterruptPoll(handles, ARRAY_SIZE(handles), 0, 500000);
InterruptPollDone(data);
}
if (flag & 0x20) {
dumpData();
}
}
// Close Motion Library
CALL_CHECK_N_RETURN_ERROR( inv_dmp_close() );
CALL_CHECK_N_RETURN_ERROR( inv_serial_stop() );
CALL_N_CHECK(IntClose(handles, ARRAY_SIZE(handles)));
return INV_SUCCESS;
}
