/**
* Runs the pedometer by polling the step counter.
*/
void PedometerLpExample(unsigned short platform,
unsigned short accelid,
unsigned short compassid,
int *handles,
int numHandles)
{
unsigned long lastStep = 0;
unsigned long lastWalkTime = 0;
unsigned int state = PED_POWER_LOW;
unsigned int pre_freeze_state = state;
int mode;
int exit = FALSE;
tMLError result;
struct mpuirq_data **data;
lastStep = 0;
stepCount = 0;
walkTime = 0;
MPL_LOGI("\n\nPerforming Pedometer\n\n");
MPL_LOGI("You should see:\n"
"\t"
"pedometer step count incrementing as steps are taken\n"
"\n");
MPL_LOGI("Loading example...\n");
MPL_LOGI("Select sensitivity:\n");
MPL_LOGI(" 0) Low (less steps)\n");
MPL_LOGI(" 1) Normal (more steps)\n");
MPL_LOGI(" 2) High (most steps)\n");
scanf("%d", &mode);
PrintPedometerMenu();
InitPedLowPower();
if (mode == 0) {
params.threshold = 30000000L;
params.minUpTime = 320; // 3.125 Hz maximum
params.maxUpTime = 1200; // 0.833 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x10000000L;
params.maxStepBufferTime = 2000;
params.clipThreshold = 0x06000000L;
} else if (mode == 1) {
params.threshold = 25000000L;
params.minUpTime = 320; // 3.125 Hz maximum
params.maxUpTime = 1200; // 0.833 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x0d000000L;
params.maxStepBufferTime = 2500;
params.clipThreshold = 0x06000000L;
} else {
params.threshold = 20000000L;
params.minUpTime = 200; // 5.00 Hz maximum
params.maxUpTime = 1500; // 0.66 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x0a000000L;
params.maxStepBufferTime = 3000;
params.clipThreshold = 0x06000000L;
}
CHECK_WARNING(MLPedometerStandAloneSetParams(¶ms));
MPL_LOGI("Example Loaded\n");
MPL_LOGI("\n");
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(0));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
state = PED_POWER_LOW;
pre_freeze_state = state;
//Loop until a key is hit
while (!exit) {
if (state == PED_POWER_LOW) {
CHECK_WARNING(MLPedometerStandAloneGetWalkTime(&walkTime));
result = MLPedometerStandAloneGetNumOfSteps(&stepCount);
if (result == ML_SUCCESS) {
if ((walkTime != lastWalkTime) || (stepCount != lastStep)) {
lastStep = stepCount;
lastWalkTime = walkTime;
MPL_LOGI("Step %6lu, Time %8.3f s\n",
lastStep,
(float)walkTime / 1000.);
}
}
// No Motion tracking
if (0) {
unsigned char data[16];
CHECK_WARNING(MLSLSerialRead(MLSerialGetHandle(),
0x68, MPUREG_23_RSVD, 6, data));
MPL_LOGI("%02x%02x, %02x%02x, %02x%02x\n",
data[0], data[1],
data[2], data[3],
data[4], data[5]);
}
}
data = InterruptPoll(handles, numHandles, 2, 50000);
/* handle system suspend/resume */
#ifdef LINUX
if (data[4]->interruptcount) {
unsigned long power_state = *((unsigned long *)data[4]->data);
if (power_state == MPU_PM_EVENT_SUSPEND_PREPARE &&
state == PED_POWER_FULL) {
pre_freeze_state = PED_POWER_FULL;
TransitionToLowPower(handles, numHandles);
} else if (power_state == MPU_PM_EVENT_POST_SUSPEND &&
pre_freeze_state == PED_POWER_FULL) {
TransitionToFullPower(handles, numHandles);
}
ioctl(handles[4], MPU_PM_EVENT_HANDLED, 0);
}
#endif
InterruptPollDone(data);
if (state == PED_POWER_SLEEP && MLDLGetIntTrigger(INTSRC_AUX1)) {
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(stepCount));
CHECK_WARNING(MLPedometerStandAloneSetWalkTime(walkTime));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
state = PED_POWER_LOW;
pre_freeze_state = state;
}
if (state == PED_POWER_LOW && MLDLGetIntTrigger(INTSRC_AUX1)) {
CHECK_WARNING(MLPedometerStandAloneGetWalkTime(&walkTime));
CHECK_WARNING(MLPedometerStandAloneGetNumOfSteps(&stepCount));
MPL_LOGI("\n");
MPL_LOGI("**** MPU SLEEP POWER ****\n");
CHECK_WARNING(MLDmpPedometerStandAloneStop());
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
state = PED_POWER_SLEEP;
pre_freeze_state = state;
}
if (state == PED_POWER_LOW && MLDLGetIntTrigger(INTSRC_MPU)) {
CHECK_WARNING(MLUpdateData());
DumpDataLowPower();
MLDLClearIntTrigger(INTSRC_MPU);
}
if (state == PED_POWER_FULL &&
(MLDLGetIntTrigger(INTSRC_MPU) || MLDLGetIntTrigger(INTSRC_AUX1))) {
CHECK_WARNING(MLUpdateData());
dumpData();
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
}
if(ConsoleKbhit()) {
char ch;
switch (state) {
case PED_POWER_SLEEP:
case PED_POWER_LOW:
/* Transition to full power */
ch = ConsoleGetChar();
switch (ch) {
case 'q':
exit = TRUE;
break;
case 'h':
PrintPedometerMenu();
break;
case 'g':
minSteps += 2;
case 'G':
--minSteps;
if (minSteps < 0)
minSteps = 0;
result = MLPedometerStandAloneSetStepBuffer(minSteps);
MPL_LOGI("MLPedometerStandAloneSetStepBuffer(%d)\n",
minSteps);
break;
case 'v':
minStepsTime += 400;
case 'V':
minStepsTime -= 200;
if (minStepsTime < 0)
minStepsTime = 0;
MLPedometerStandAloneSetStepBufferResetTime(minStepsTime);
MPL_LOGI(
"MLPedometerStandAloneSetStepBufferResetTime(%d)\n",
minStepsTime);
break;
case 'b':
flag ^= 0x20;
if (flag & 0x20) {
FIFOSendAccel(ML_ELEMENT_1 |
ML_ELEMENT_2 |
ML_ELEMENT_3,
ML_32_BIT);
MLSetFifoInterrupt(TRUE);
} else {
FIFOSendAccel(ML_ELEMENT_1 |
ML_ELEMENT_2 |
ML_ELEMENT_3,
0);
MLSetFifoInterrupt(FALSE);
}
break;
case '\n':
case '\r':
/* Ignore carrage return and line feeds */
break;
default:
TransitionToFullPower(handles,numHandles);
state = PED_POWER_FULL;
pre_freeze_state = state;
break;
};
break;
case PED_POWER_FULL:
default:
/* Transition to low power */
exit = ProcessKbhit();
if (exit) {
TransitionToLowPower(handles, numHandles);
state = PED_POWER_LOW;
pre_freeze_state = state;
exit = FALSE;
}
break;
};
}
}
if (state == PED_POWER_LOW) {
CHECK_WARNING(MLDmpPedometerStandAloneClose());
} else if (PED_POWER_FULL == state) {
CHECK_WARNING(MLDmpClose());
}
}
// 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;
}