/**
* @brief umplStartAccelOnlyLowPower starts the accelerometer
* in low power mode.
*
* @pre umplStartMPU() must have been called.
* @param freq
* The update rate can be 40 Hz, 10 Hz, 2.5 Hz, 1.25 Hz.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartAccelOnlyLowPower(float freq)
{
int result;
if(umplState == UMPL_STOP)
{
umplStartMPU();
}
if (umplState == UMPL_RUN)
{
fifo_rate = inv_get_fifo_rate();
}
if (umplState == UMPL_RUN || umplState == UMPL_ACCEL_ONLY)
{
if( freq <= 1.25 )
inv_set_fifo_rate(159); //fifo rate = 1.25 hz
else if( freq <= 2.5 && freq > 1.25 )
inv_set_fifo_rate(79); //fifo rate = 2.5 hz
else if( freq <= 10.0 && freq > 2.5 )
inv_set_fifo_rate(19); //fifo rate = 10 hz
else
inv_set_fifo_rate(4); //fifo rate = 40 hz
inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
umplSetState(UMPL_LPACCEL_ONLY);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/**
* @brief umplStartAccelOnly starts the accelerometer.
*
* @pre umplStartMPU() must have been called.
* @param freq
* The update rate can be 18 Hz, 50 Hz and 100 Hz.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartAccelOnly(float freq)
{
int result;
if(umplState == UMPL_STOP)
{
umplStartMPU();
}
if (umplState == UMPL_RUN)
{
fifo_rate = inv_get_fifo_rate();
}
if (umplState == UMPL_RUN || umplState == UMPL_LPACCEL_ONLY)
{
if( freq <= 18.0 )
inv_set_fifo_rate(10); //fifo rate = 18 hz
else if( freq <= 50.0 && freq > 18.0 )
inv_set_fifo_rate(3); //fifo rate = 50 hz
else
inv_set_fifo_rate(1); //fifo rate = 100 hz
inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
umplSetState(UMPL_ACCEL_ONLY);
return INV_SUCCESS;
}
return INV_ERROR_SM_IMPROPER_STATE;
}
inv_error_t umplDmpSetup(void)
{
inv_error_t result;
/* Setup Bias Trackers:
* inv_set_bias_update
* Known to work with the following arguments:
* INV_BIAS_FROM_NO_MOTION
* INV_BIAS_FROM_TEMPERATURE
* INV_LEARN_BIAS_FROM_TEMPERATURE
*
*/
#if defined UMPL_NINE_AXIS
inv_set_mpu_sensors(INV_NINE_AXIS);
#else
inv_set_mpu_sensors(INV_SIX_AXIS_GYRO_ACCEL);
#endif
#ifdef UMPL_ENABLE_TEMPCOMP
result = inv_set_bias_update( INV_BIAS_FROM_NO_MOTION
| INV_BIAS_FROM_TEMPERATURE
| INV_LEARN_BIAS_FROM_TEMPERATURE );
#else
result = inv_set_bias_update( INV_BIAS_FROM_NO_MOTION );
#endif
if (result != INV_SUCCESS)
#ifdef UMPL_NINE_AXIS
result = inv_external_slave_akm8975_open();
if (result != INV_SUCCESS)
#endif
inv_set_fifo_interrupt(1);
// Setup FIFO:
result = inv_send_quaternion(INV_32_BIT);
if (result != INV_SUCCESS)
result = inv_send_gyro(INV_ALL,INV_32_BIT);
if (result != INV_SUCCESS)
result = inv_send_accel(INV_ALL,INV_32_BIT);
if (result != INV_SUCCESS)
result = inv_set_fifo_rate(10);
if (result != INV_SUCCESS)
#ifdef UMPL_ENABLE_TEMPCOMP
result = inv_enable_temp_comp();
if (result != INV_SUCCESS)
#endif
result = inv_set_motion_callback(umplMotionCallback);
if (result != INV_SUCCESS)
return INV_SUCCESS;
}
/**
* @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;
}
int MPLSensor::update_delay()
{
FUNC_LOG;
int rv = 0;
bool irq_set[5];
pthread_mutex_lock(&mMplMutex);
if (mEnabled) {
uint64_t wanted = -1LLU;
for (int i = 0; i < numSensors; i++) {
if (mEnabled & (1 << i)) {
uint64_t ns = mDelays[i];
wanted = wanted < ns ? wanted : ns;
}
}
//Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns
if (wanted < 10000000LLU) {
wanted = 10000000LLU;
}
int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1
: 0); //mpu fifo rate is in increments of 5ms
if (rate == 0) //KLP disallow fifo rate 0
rate = 1;
if (rate != mCurFifoRate) {
//ALOGD("set fifo rate: %d %llu", rate, wanted);
inv_error_t res; // = inv_dmp_stop();
res = inv_set_fifo_rate(rate);
ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
//res = inv_dmp_start();
//ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP");
mCurFifoRate = rate;
rv = (res == INV_SUCCESS);
}
if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) {
if (mUseTimerirq) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
clearIrqData(irq_set);
if (inv_get_dl_config()->requested_sensors
== INV_THREE_AXIS_COMPASS) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
(unsigned long) (wanted / 1000000LLU));
ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
(int) (wanted / 1000000LLU));
} else {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
(unsigned long) inv_get_sample_step_size_ms());
ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
(int) inv_get_sample_step_size_ms());
}
}
}
}
pthread_mutex_unlock(&mMplMutex);
return rv;
}
/**
* container function for all the calls we make once to set up the MPL.
*/
void MPLSensor::initMPL()
{
FUNC_LOG;
inv_error_t result;
unsigned short bias_update_mask = 0xFFFF;
struct mldl_cfg *mldl_cfg;
if (inv_dmp_open() != INV_SUCCESS) {
ALOGE("Fatal Error : could not open DMP correctly.\n");
}
result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work
ALOGE_IF(result != INV_SUCCESS,
"Fatal Error : could not set enabled sensors.");
if (inv_load_calibration() != INV_SUCCESS) {
ALOGE("could not open MPL calibration file");
}
//check for the 9axis fusion library: if available load it and start 9x
void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW);
if(h_dmp_lib) {
const char* error;
error = dlerror();
inv_error_t (*fp_inv_enable_9x_fusion)() =
(inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion");
if((error = dlerror()) != NULL) {
ALOGE("%s %s", error, "inv_enable_9x_fusion");
} else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) {
ALOGE( "Warning : 9 axis sensor fusion not available "
"- No compass detected.\n");
} else {
/* 9axis is loaded and enabled */
/* this variable is used for coming up with sensor list */
mNineAxisEnabled = true;
}
} else {
const char* error = dlerror();
ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error);
}
mldl_cfg = inv_get_dl_config();
if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
ALOGE("Error : Bias update function could not be set.\n");
}
if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
ALOGE("Error : could not set motion interrupt");
}
if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
ALOGE("Error : could not set fifo interrupt");
}
result = inv_set_fifo_rate(6);
if (result != INV_SUCCESS) {
ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
}
setupCallbacks();
}
// 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;
}
