/**
* @brief An MPL wrapper for the main MPU Self Test API inv_factory_calibrate().
* See inv_factory_calibrate() function for more details.
*
* @pre inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg
* data structure.
* On Windows, SetupPlatform() is also a madatory pre condition to
* ensure the accelerometer is properly configured before running the
* test.
*
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param provide_result
* If 1:
* perform and analyze the offset, drive frequency, and noise
* calculation and compare it against set thresholds. Report
* also the final result using a bit-mask like error code as
* described in the inv_test_gyro_xxxx() functions.
* When 0:
* skip the noise and drive frequency calculation and pass/fail
* assessment. It simply calculates the gyro and accel biases.
* NOTE: for MPU6050 devices, this parameter is currently
* ignored.
*
* @return INV_SUCCESS or first non-zero error code otherwise.
*/
inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle,
unsigned char provide_result)
{
INVENSENSE_FUNC_START;
inv_error_t firstError = INV_SUCCESS;
inv_error_t result;
unsigned char initState = inv_get_state();
if (initState < INV_STATE_DMP_OPENED) {
MPL_LOGE("Self Test cannot run before inv_dmp_open()\n");
return INV_ERROR_SM_IMPROPER_STATE;
}
/* obtain a pointer to the 'struct mldl_cfg' data structure. */
mputestCfgPtr = inv_get_dl_config();
if(initState == INV_STATE_DMP_STARTED) {
result = inv_dmp_stop();
ERROR_CHECK_FIRST(firstError, result);
}
result = inv_factory_calibrate(mlsl_handle, provide_result);
ERROR_CHECK_FIRST(firstError, result);
if(initState == INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
ERROR_CHECK_FIRST(firstError, result);
}
return firstError;
}
/**
* @brief umplStartMPU kicks starts the uMPL state machine. This function
* in turn calls the MPL functions like inv_dmp_open() and inv_dmp_start() which starts
* the motion processing algorithms. This function also enables the required features
* such as turning on the bias trackers and temperature compensation.
* This function enables the required type of data to be put in FIFO.
*
*
*
* @pre umplInit() must have been called.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartMPU(void)
{
inv_error_t result;
if (umplState == UMPL_STOP)
{
MPL_LOGV("UMPL_STOP to UMPL_RUN\n");
result = inv_dmp_open();
if (result != INV_SUCCESS) return result;
result = umplDmpSetup();
if (result != INV_SUCCESS) return result;
result = inv_dmp_start();
if (result != INV_SUCCESS) return result;
#ifndef UMPL_DISABLE_LOAD_CAL
result = inv_uload_calibration();
if (result != INV_SUCCESS)
MPL_LOGE("inv_uload_calibration failed with %d in umplStartMPU\n",result);
#endif
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_ACCEL_ONLY )
{
struct mldl_cfg * mldl_cfg = inv_get_dl_config();
MPL_LOGD("UMPL_ACCEL_ONLY (or UMPL_LPACCEL_ONLY) to UMPL_RUN\n");
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) {
inv_set_mpu_sensors( INV_NINE_AXIS );
} else {
inv_set_mpu_sensors( INV_SIX_AXIS_GYRO_ACCEL );
}
inv_set_fifo_rate(fifo_rate);
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_LPACCEL_ONLY )
{
umplStartAccelOnly(0.0);
umplStartMPU();
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/* set the power states of the various sensors based on the bits set in the
* enabled_sensors parameter.
* this function modifies globalish state variables. It must be called with the mMplMutex held. */
void MPLSensor::setPowerStates(int enabled_sensors)
{
FUNC_LOG;
bool irq_set[5] = { false, false, false, false, false };
//ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted);
do {
if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
mLocalSensorMask = ALL_MPL_SENSORS_NP;
break;
}
if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
mLocalSensorMask = 0;
break;
}
if (GY_ENABLED) {
mLocalSensorMask |= INV_THREE_AXIS_GYRO;
} else {
mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
}
if (A_ENABLED) {
mLocalSensorMask |= (INV_THREE_AXIS_ACCEL);
} else {
mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL);
}
if (M_ENABLED) {
mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
} else {
mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS);
}
} while (0);
//record the new sensor state
inv_error_t rv;
long sen_mask = mLocalSensorMask & mMasterSensorMask;
bool changing_sensors = ((inv_get_dl_config()->requested_sensors
!= sen_mask) && (sen_mask != 0));
bool restart = (!mDmpStarted) && (sen_mask != 0);
if (changing_sensors || restart) {
ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart);
if (mDmpStarted) {
inv_dmp_stop();
clearIrqData(irq_set);
mDmpStarted = false;
}
if (sen_mask != inv_get_dl_config()->requested_sensors) {
//ALOGV("setPowerStates: %lx", sen_mask);
rv = inv_set_mpu_sensors(sen_mask);
ALOGE_IF(rv != INV_SUCCESS,
"error: unable to set MPL sensor power states (sens=%ld retcode = %d)",
sen_mask, rv);
}
if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS))
|| (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL)))
&& ((sen_mask & INV_DMP_PROCESSOR) == 0)) {
ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
mUseTimerirq = true;
} else {
if (mUseTimerirq) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
clearIrqData(irq_set);
}
ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
mUseTimerirq = false;
}
if (!mDmpStarted) {
if (mHaveGoodMpuCal || mHaveGoodCompassCal) {
rv = inv_store_calibration();
ALOGE_IF(rv != INV_SUCCESS,
"error: unable to store MPL calibration file");
mHaveGoodMpuCal = false;
mHaveGoodCompassCal = false;
}
//ALOGV("Starting DMP");
rv = inv_dmp_start();
ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
mDmpStarted = true;
}
}
//check if we should stop the DMP
if (mDmpStarted && (sen_mask == 0)) {
//ALOGV("Stopping DMP");
rv = inv_dmp_stop();
ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)",
rv);
if (mUseTimerirq) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
}
clearIrqData(irq_set);
mDmpStarted = false;
mPollTime = -1;
mCurFifoRate = -1;
}
}
/**
* @brief The main entry point of the MPU Self Test, triggering the run of
* the single tests, for gyros and accelerometers.
* Prepares the MPU for the test, taking the device out of low power
* state if necessary, switching the MPU secondary I2C interface into
* bypass mode and restoring the original power state at the end of
* the test.
* This function is also responsible for encoding the output of each
* test in the correct format as it is stored on the file/medium of
* choice (according to inv_serial_write_cal() function).
* The format needs to stay perfectly consistent with the one expected
* by the corresponding loader in ml_stored_data.c; currectly the
* loaded in use is inv_load_cal_V1 (record type 1 - initial
* calibration).
*
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param perform_full_test
* If 1:
* Complete calibration test:
* Calculate offset, drive frequency, and noise and compare it
* against set thresholds.
* When 0:
* Skip the noise and drive frequency calculation,
* simply calculate the gyro biases.
* @param provide_result
* If 1:
* Report the final result using a bit-mask like error code as
* described in the test_gyro() function.
*
* @return 0 on success. A non-zero error code on error.
* Propagates the errors from the tests up to the caller.
*/
int inv_device_test(void *mlsl_handle,
uint_fast8_t sensor_mask,
uint_fast8_t perform_full_test,
uint_fast8_t provide_result)
{
int result = 0, gyro_test_result = 0, accel_test_result = 0;
short temp_avg = 0;
short gyro_biases[3] = {0, 0, 0};
short accel_biases[3] = {0, 0, 0};
long accel_sens[3] = {0};
unsigned long saved_sensor_mask;
unsigned char saved_state = inv_get_state();
mldl_cfg = inv_get_dl_config();
saved_sensor_mask = mldl_cfg->inv_mpu_cfg->requested_sensors;
if (sensor_mask & (INV_THREE_AXIS_GYRO & ~INV_DMP_PROCESSOR)) {
result = inv_set_mpu_sensors(INV_THREE_AXIS_GYRO & ~INV_DMP_PROCESSOR);
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
if (saved_state < INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
}
#ifdef TRACK_IDS
result = get_mpu_unique_id(mlsl_handle);
if (result != INV_SUCCESS) {
MPL_LOGI("Could not read the device's unique ID\n");
MPL_LOGI("\n");
return -1;
}
#endif
MPL_LOGI("Collecting one group of %d ms samples for each axis\n",
(perform_full_test ? DEF_PERIOD_CAL : DEF_PERIOD_SELF));
MPL_LOGI("\n");
/* adjust the gyro sensitivity according to the gyro_sens_trim value */
adj_gyro_sens = test_setup.gyro_sens *
mldl_cfg->mpu_chip_info->gyro_sens_trim / 131.072f;
test_setup.gyro_fs = (int)(32768.f / adj_gyro_sens);
/* collect gyro and temperature data, test gyro, report result */
gyro_test_result = test_gyro(mlsl_handle,
gyro_biases, &temp_avg, perform_full_test);
MPL_LOGI("\n");
if (gyro_test_result == 0) {
MPL_LOGI_IF(provide_result, "Test : PASSED\n");
} else {
MPL_LOGI_IF(provide_result, "Test : FAILED %d/%04X - Biases NOT stored\n",
gyro_test_result, gyro_test_result);
goto gyro_test_failed;
}
MPL_LOGI_IF(provide_result, "\n");
}
if (sensor_mask & INV_THREE_AXIS_ACCEL) {
if (!mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]) {
MPL_LOGI("\n");
MPL_LOGI("No accelerometer configured\n");
} else {
result = inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
if (result)
return -1;
if (inv_get_state() < INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
if (result)
return -1;
}
get_accel_sensitivity(accel_sens);
/* collect accel data. if this step is skipped,
ensure the array still contains zeros. */
accel_test_result =
test_accel(mlsl_handle, sensor_mask >> 4,
accel_biases, accel_sens[Z],
perform_full_test);
if (accel_test_result)
goto accel_test_failed;
/* if only Z accel is requested,
clear out the biases from the other 2 axes */
if ((sensor_mask & INV_THREE_AXIS_ACCEL) == INV_Z_ACCEL)
accel_biases[X] = accel_biases[Y] = 0;
}
}
ALOGE("in %s: accel_bias is %d %d %d", __func__, accel_biases[0],
accel_biases[1], accel_biases[2]);
result = populate_data_store(sensor_mask, temp_avg, gyro_biases,
accel_biases, accel_sens);
if (result)
return -1;
result = inv_serial_write_cal(data_store, ML_INIT_CAL_LEN);
if (result) {
MPL_LOGI("Error : cannot write calibration on file - error %d\n",
result);
LOG_RESULT_LOCATION(result);
return -1;
}
gyro_test_failed:
accel_test_failed:
/* restore the setting had at the beginning */
mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
result = inv_set_mpu_sensors(saved_sensor_mask);
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
/* turn off only if it was off when the function was called */
if (saved_state < INV_STATE_DMP_STARTED) {
result = inv_dmp_stop();
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
}
if (gyro_test_result)
return gyro_test_result;
if (accel_test_result)
return accel_test_result;
return result;
}
// 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;
}