/** * @brief Get a sample of pressure data from the device. * @param data * the buffer to store the pressure raw data for * X, Y, and Z axes. * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_get_pressure_data(long *data) { inv_error_t result = INV_SUCCESS; unsigned char tmp[3]; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); /*--- read the pressure sensor data. The pressure read function may return an INV_ERROR_PRESSURE_* errors when the data is not ready (read/refresh frequency mismatch) or the internal data sampling timing of the device was not respected. Returning the error code will make the sensor fusion supervisor ignore this pressure data sample. ---*/ result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), tmp); if (result) { _pressureDebug(MPL_LOGV ("inv_mpu_read_pressure returned %d (%s)\n", result, MLErrorCode(result))); return result; } if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE]->endian) data[0] = (((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) + ((long)tmp[2]); else data[0] = (((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) + ((long)tmp[0]); return INV_SUCCESS; }
inv_error_t inv_compass_check_range(void) { struct ext_slave_config config; unsigned char data[3]; inv_error_t result; config.key = MPU_SLAVE_RANGE_CHECK; config.len = 3; config.apply = TRUE; config.data = data; result = inv_mpu_get_compass_config(inv_get_dl_config(), inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } if(data[0] || data[1] || data[2]) { /* some value clipped */ return INV_ERROR_COMPASS_DATA_ERROR; } return INV_SUCCESS; }
/** * @brief Runs the MPU test at MPL runtime. * If the DMP is operating, stops the DMP temporarely, * runs the MPU Self Test, and re-starts the DMP. * * @return INV_SUCCESS or a non-zero error code otherwise. */ inv_error_t inv_self_test_run(void) { #ifdef CONFIG_MPU_SENSORS_MPU3050 return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE); #else return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); #endif }
/** * @brief Get a sample of accel data from the device. * @param data * the buffer to store the accel raw data for * X, Y, and Z axes. * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_get_accel_data(long *data) { struct mldl_cfg *mldl_cfg = inv_get_dl_config(); inv_error_t result; unsigned char raw_data[2 * ACCEL_NUM_AXES]; long tmp[ACCEL_NUM_AXES]; int ii; signed char *mtx = mldl_cfg->pdata->accel.orientation; char accelId = mldl_cfg->accel->id; if (NULL == data) return INV_ERROR_INVALID_PARAMETER; if (mldl_cfg->accel->read_len > sizeof(raw_data)) return INV_ERROR_ASSERTION_FAILURE; result = (inv_error_t) inv_mpu_read_accel(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), raw_data); if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { return result; } if (result) { LOG_RESULT_LOCATION(result); return result; } for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) { tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256; tmp[ii] += (long)((unsigned char)raw_data[2 * ii]); } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) || (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) { tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256; tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]); if (accelId == ACCEL_ID_KXSD9) { tmp[ii] = (long)((short)(((unsigned short)tmp[ii]) + ((unsigned short)0x8000))); } } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) { tmp[ii] = (long)((signed char)raw_data[ii]) * 256; } else { result = INV_ERROR_FEATURE_NOT_IMPLEMENTED; } } for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { data[ii] = ((long)tmp[0] * mtx[3 * ii] + (long)tmp[1] * mtx[3 * ii + 1] + (long)tmp[2] * mtx[3 * ii + 2]); } //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]); return result; }
/** * @brief Get a sample of compass data from the device. * @param data * the buffer to store the compass raw data for * X, Y, and Z axes. * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_get_compass_data(long *data) { inv_error_t result; int ii; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); unsigned char *tmp = inv_obj.mag->raw_data; if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len > sizeof(inv_obj.mag->raw_data)) { LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); return INV_ERROR_INVALID_CONFIGURATION; } if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus == EXT_SLAVE_BUS_PRIMARY || !(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) { /*--- read the compass sensor data. The compass read function may return an INV_ERROR_COMPASS_* errors when the data is not ready (read/refresh frequency mismatch) or the internal data sampling timing of the device was not respected. Returning the error code will make the sensor fusion supervisor ignore this compass data sample. ---*/ result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), tmp); if (result) { if (COMPASS_DEBUG) { MPL_LOGV("inv_mpu_read_compass returned %d\n", result); } return result; } for (ii = 0; ii < 3; ii++) { if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian) data[ii] = ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; else data[ii] = ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; } } else { /* compass on the 2nd bus or DMP is off */ result = inv_get_external_sensor_data(data, 3); if (result) { LOG_RESULT_LOCATION(result); return result; } } data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]); data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]); data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]); return INV_SUCCESS; }
inv_error_t inv_set_compass_offset(void) { struct ext_slave_config config; unsigned char data[3]; inv_error_t result; config.key = MPU_SLAVE_OFFSET_VALS; config.len = 3; config.apply = TRUE; config.data = data; if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) { /* push stored values */ data[0] = (char)inv_obj.compass_offsets[0]; data[1] = (char)inv_obj.compass_offsets[1]; data[2] = (char)inv_obj.compass_offsets[2]; MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]); result = inv_mpu_config_compass(inv_get_dl_config(), inv_get_serial_handle(), inv_get_serial_handle(), &config); } else { /* compute new values and store them */ result = inv_mpu_get_compass_config(inv_get_dl_config(), inv_get_serial_handle(), inv_get_serial_handle(), &config); MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]); if(result == INV_SUCCESS) { inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1; inv_obj.compass_offsets[0] = data[0]; inv_obj.compass_offsets[1] = data[1]; inv_obj.compass_offsets[2] = data[2]; } } if (result) { LOG_RESULT_LOCATION(result); return result; } return result; }
/** * @brief Read values from the compass slave device scale registers, * regardless of the bus it is connected to and the MPU's configuration. * @param reg * the register to read from on the slave compass device. * @param val * a buffer of 3 elements to store the values read from the * compass device. * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. */ inv_error_t inv_compass_read_scale(long *val) { struct ext_slave_config config; unsigned char data[3]; inv_error_t result; config.key = MPU_SLAVE_READ_SCALE; config.len = 3; config.apply = true; config.data = data; result = inv_mpu_get_compass_config(inv_get_dl_config(), inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } val[0] = ((long)(data[0] - 128) << 22) + (1L << 30); val[1] = ((long)(data[1] - 128) << 22) + (1L << 30); val[2] = ((long)(data[2] - 128) << 22) + (1L << 30); return result; }
/** * @brief Read values from the compass slave device registers, regardless * of the bus it is connected to and the MPU's configuration. * @param reg * the register to read from on the slave compass device. * @param val * a buffer of 3 elements to store the values read from the * compass device. * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. */ inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val) { struct ext_slave_config config; unsigned char data[2]; inv_error_t result; data[0] = reg; config.key = MPU_SLAVE_READ_REGISTERS; config.len = 2; config.apply = true; config.data = data; result = inv_mpu_get_compass_config(inv_get_dl_config(), inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } *val = data[1]; return result; }
/** * @brief Write a single register on the compass slave device, regardless * of the bus it is connected to and the MPU's configuration. * @param reg * the register to write to on the slave compass device. * @param val * the value to write. * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. */ inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val) { struct ext_slave_config config; unsigned char data[2]; inv_error_t result; data[0] = reg; data[1] = val; config.key = MPU_SLAVE_WRITE_REGISTERS; config.len = 2; config.apply = TRUE; config.data = data; result = inv_mpu_config_compass(inv_get_dl_config(), inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; }
void MPLSensor::handlePowerEvent() { VFUNC_LOG; mpuirq_data irqd; int fd = (uintptr_t) inv_get_serial_handle(); read(fd, &irqd, sizeof(irqd)); if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { //going to sleep sleepEvent(); } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { //waking up wakeEvent(); } ioctl(fd, MPU_PM_EVENT_HANDLED, 0); }
int MPLSensor::getPowerFd() const { int hdl = (uintptr_t) inv_get_serial_handle(); //ALOGV("MPLSensor::getPowerFd returning %d", hdl); return hdl; }
/** * @brief * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_setup_dmp(void) { inv_error_t result; struct mldl_cfg * mldl_cfg = inv_get_dl_config(); inv_set_get_address( inv_setup_dmpGetAddress ); result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_set_full_scale(2000.f); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_load_dmp(dmpMemory, DMP_CODE_SIZE, sConfig); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_set_ignore_system_suspend(false); if (result) { LOG_RESULT_LOCATION(result); return result; } if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] && mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->config) { struct ext_slave_config config; long odr; config.key = MPU_SLAVE_CONFIG_ODR_SUSPEND; config.len = sizeof(long); config.apply = false; config.data = &odr; odr = 0; result = inv_mpu_config_accel(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } config.key = MPU_SLAVE_CONFIG_ODR_RESUME; odr = 200000; result = inv_mpu_config_accel(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } config.key = MPU_SLAVE_CONFIG_IRQ_SUSPEND; odr = MPU_SLAVE_IRQ_TYPE_NONE; result = inv_mpu_config_accel(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; odr = MPU_SLAVE_IRQ_TYPE_NONE; result = inv_mpu_config_accel(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), &config); if (result) { LOG_RESULT_LOCATION(result); return result; } } return result; }
/** * @brief Runs the MPU test for bias correction only at MPL runtime. * If the DMP is operating, stops the DMP temporarely, * runs the bias calculation routines, and re-starts the DMP. * * @return INV_SUCCESS or a non-zero error code otherwise. */ inv_error_t inv_self_test_bias_only(void) { return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); }
/** * @brief Get a sample of compass data from the device. * @param data * the buffer to store the compass raw data for * X, Y, and Z axes. * @return INV_SUCCESS or a non-zero error code. */ inv_error_t inv_get_compass_data(long *data) { inv_error_t result; int ii; struct mldl_cfg *mldl_cfg = inv_get_dl_config(); unsigned char *tmp = inv_obj.mag->raw_data; if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len > sizeof(inv_obj.mag->raw_data)) { LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); return INV_ERROR_INVALID_CONFIGURATION; } if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus == EXT_SLAVE_BUS_PRIMARY || !(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) { /*--- read the compass sensor data. The compass read function may return an INV_ERROR_COMPASS_* errors when the data is not ready (read/refresh frequency mismatch) or the internal data sampling timing of the device was not respected. Returning the error code will make the sensor fusion supervisor ignore this compass data sample. ---*/ result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, inv_get_serial_handle(), inv_get_serial_handle(), tmp); if (result) { if (COMPASS_DEBUG) { MPL_LOGV("inv_mpu_read_compass returned %d\n", result); } return result; } for (ii = 0; ii < 3; ii++) { if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian) data[ii] = ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; else data[ii] = ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; } } else { #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 result = inv_get_external_sensor_data(data, 3); if (result) { LOG_RESULT_LOCATION(result); return result; } #if defined CONFIG_MPU_SENSORS_MPU6050A2 { static unsigned char first = true; // one-off write to AKM if (first) { unsigned char regs[] = { // beginning Mantis register for one-off slave R/W MPUREG_I2C_SLV4_ADDR, // the slave to write to mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->address, // the register to write to /*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->reg */ 0x0A, // the value to write /*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->value */ 0x01, // enable the write 0xC0 }; result = inv_serial_write(inv_get_serial_handle(), mldl_cfg->mpu_chip_info->addr, ARRAY_SIZE(regs), regs); first = false; } else { unsigned char regs[] = { MPUREG_I2C_SLV4_CTRL, 0xC0 }; result = inv_serial_write(inv_get_serial_handle(), mldl_cfg->mpu_chip_info->addr, ARRAY_SIZE(regs), regs); } } #endif #else return INV_ERROR_INVALID_CONFIGURATION; #endif // CONFIG_MPU_SENSORS_xxxx } data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]); data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]); data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]); return INV_SUCCESS; }
/** * @internal * @brief used to get the FIFO data. * @param length * Number of bytes to read from the FIFO. * @param buffer * the bytes of FIFO data. * Note that this buffer <b>must</b> be large enough * to store and additional trailing FIFO footer when * expected. The callers must make sure enough space * is allocated. * @return number of valid bytes of data. **/ uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer) { INVENSENSE_FUNC_START; inv_error_t result; uint_fast16_t inFifo; uint_fast16_t toRead; int_fast8_t kk; toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount; /*---- make sure length is correct ----*/ if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) { fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER; return 0; } result = inv_get_fifo_length(&inFifo); if (INV_SUCCESS != result) { fifo_objHW.fifoError = result; return 0; } // fifo_objHW.fifoCount is the footer size left in the buffer, or // 0 if this is the first time reading the fifo since it was reset if (inFifo < length + fifo_objHW.fifoCount) { fifo_objHW.fifoError = INV_SUCCESS; return 0; } // if a trailing fifo count is expected - start storing data 2 bytes before result = inv_read_fifo(fifo_objHW.fifoCount > 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead); if (INV_SUCCESS != result) { fifo_objHW.fifoError = result; return 0; } // Make sure the fifo didn't overflow before or during the read result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow); if (INV_SUCCESS != result) { fifo_objHW.fifoError = result; return 0; } if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) { MPL_LOGV("Resetting Fifo : Overflow\n"); inv_reset_fifo(); fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW; return 0; } /* Check the Footer value to give us a chance at making sure data * didn't get corrupted */ for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) { if (buffer[kk] != gFifoFooter[kk]) { MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n", buffer[0], buffer[1]); _fifoDebug(char out[200]; MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount); sprintf(out, "0x"); for (kk = 0; kk < (int)toRead; kk++) { sprintf(out, "%s%02X", out, buffer[kk]);} MPL_LOGW("%s\n", out);) inv_reset_fifo(); fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER; return 0; }
// 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; }