void parse_events(struct pollfd pfd[], int num_fds)
{
int i;
unsigned long now;
for (i = 0; i < num_fds; i++) {
if(pfd[i].revents != 0) {
switch(i) {
case FEAT_TAP:
tap_handler();
break;
case FEAT_SCREEN_ORIENTATION:
display_orientation_handler();
break;
/*
case FEAT_ORIENTATION:
host_orientation_handler();
break;
*/
default:
printf("GT:ERR-unhandled/unrecognized gesture event");
break;
}
pfd[i].revents = 0; // no need: reset anyway
}
}
// pedometer is not event based, therefore we poll using a timer every
// pedometer_poll_timeout milliseconds
if ((now = inv_get_tick_count()) - last_pedometer_poll
> pedometer_poll_timeout) {
pedometer_handler();
last_pedometer_poll = now;
}
}
/**
* @brief Test the gyroscope sensor.
* Implements the core logic of the MPU Self Test.
* Produces the PASS/FAIL result. Loads the calculated gyro biases
* and temperature datum into the corresponding pointers.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param gyro_biases
* output pointer to store the initial bias calculation provided
* by the MPU Self Test. Requires 3 elements for gyro X, Y,
* and Z.
* @param temp_avg
* output pointer to store the initial average temperature as
* provided by the MPU Self Test.
* @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.
*
* @return 0 on success.
* On error, the return value is a bitmask representing:
* 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively
* (decimal values will be 1, 2, 4 respectively).
* 3, 4, 5 Excessive offset with X, Y, Z gyros respectively
* (decimal values will be 8, 16, 32 respectively).
* 6, 7, 8 Excessive noise with X, Y, Z gyros respectively
* (decimal values will be 64, 128, 256 respectively).
* 9 If any of the RMS noise values is zero, it may be
* due to a non-functional gyro or FIFO/register failure.
* (decimal value will be 512).
*/
int test_gyro(void *mlsl_handle,
short gyro_biases[3], short *temp_avg,
uint_fast8_t perform_full_test)
{
int ret_val = 0;
inv_error_t result;
int total_count = 0;
int total_count_axis[3] = {0, 0, 0};
int packet_count;
short x[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
short y[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
short z[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
int temperature = 0;
float avg[3];
float rms[3];
unsigned long test_start = inv_get_tick_count();
int i, j, tmp;
char tmpStr[200];
unsigned char regs[7] = {0};
/* make sure the DMP is disabled first */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* reset the gyro offset values */
regs[0] = MPUREG_XG_OFFS_USRH;
result = inv_serial_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
6, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* sample rate */
if (perform_full_test) {
/* = 8ms */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_SMPLRT_DIV, 0x07);
test_setup.bias_thresh = (int)(
DEF_BIAS_THRESH_CAL * test_setup.gyro_sens);
} else {
/* = 1ms */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_SMPLRT_DIV, 0x00);
test_setup.bias_thresh = (int)(
DEF_BIAS_THRESH_SELF * test_setup.gyro_sens);
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
switch (test_setup.gyro_fs) {
case 2000:
regs[0] |= 0x18;
break;
case 1000:
regs[0] |= 0x10;
break;
case 500:
regs[0] |= 0x08;
break;
case 250:
default:
regs[0] |= 0x00;
break;
}
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_CONFIG, regs[0]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_INT_ENABLE, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* 1st, timing test */
for (j = 0; j < 3; j++) {
MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
/* turn on all gyros, use gyro X for clocking
Set to Y and Z for 2nd and 3rd iteration */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_PWR_MGMT_1, j + 1);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* wait for 2 ms after switching clock source */
usleep(2000);
/* enable & reset FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
tmp = test_setup.tests_per_axis;
while (tmp-- > 0) {
const unsigned char fifo_en_reg = MPUREG_FIFO_EN;
/* enable XYZ gyro in FIFO and nothing else */
result = inv_serial_single_write(mlsl_handle,
mldl_cfg->mpu_chip_info->addr, fifo_en_reg,
BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* wait one period for data */
if (perform_full_test)
usleep(DEF_PERIOD_CAL * 1000);
else
usleep(DEF_PERIOD_SELF * 1000);
/* stop storing gyro in the FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
fifo_en_reg, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Getting number of bytes in FIFO */
result = inv_serial_read(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_FIFO_COUNTH, 2, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* number of 6 B packets in the FIFO */
packet_count = inv_big8_to_int16(dataout) / 6;
sprintf(tmpStr, "Packet Count: %d - ", packet_count);
if (abs(packet_count - test_setup.packet_thresh)
<= /* Within total_timing_tol % range, rounded up */
(int)(test_setup.total_timing_tol *
test_setup.packet_thresh + 1)) {
for (i = 0; i < packet_count; i++) {
/* getting FIFO data */
result = inv_serial_read_fifo(mlsl_handle,
mldl_cfg->mpu_chip_info->addr, 6, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
x[total_count + i] = inv_big8_to_int16(&dataout[0]);
y[total_count + i] = inv_big8_to_int16(&dataout[2]);
z[total_count + i] = inv_big8_to_int16(&dataout[4]);
if (VERBOSE_OUT) {
MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n",
total_count + i, x[total_count + i],
y[total_count + i], z[total_count + i]);
}
}
total_count += packet_count;
total_count_axis[j] += packet_count;
sprintf(tmpStr, "%sOK", tmpStr);
} else {
ret_val |= 1 << j;
sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
}
MPL_LOGI("%s\n", tmpStr);
}
/* remove gyros from FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_FIFO_EN, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Read Temperature */
result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_TEMP_OUT_H, 2, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
temperature += (short)inv_big8_to_int16(dataout);
}
MPL_LOGI("\n");
MPL_LOGI("Total %d samples\n", total_count);
MPL_LOGI("\n");
/* 2nd, check bias from X, Y, and Z PLL clock source */
tmp = total_count != 0 ? total_count : 1;
for (i = 0, avg[X] = .0f, avg[Y] = .0f, avg[Z] = .0f;
i < total_count; i++) {
avg[X] += 1.f * x[i] / tmp;
avg[Y] += 1.f * y[i] / tmp;
avg[Z] += 1.f * z[i] / tmp;
}
MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT) {
MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n",
avg[X] / adj_gyro_sens,
avg[Y] / adj_gyro_sens,
avg[Z] / adj_gyro_sens);
}
for (j = 0; j < 3; j++) {
if (fabs(avg[j]) > test_setup.bias_thresh) {
MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold "
"(threshold = %d)\n",
a_name[j], avg[j], test_setup.bias_thresh);
ret_val |= 1 << (3+j);
}
}
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
for (i = 0, rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
i < total_count; i++) {
rms[X] += (x[i] - avg[X]) * (x[i] - avg[X]);
rms[Y] += (y[i] - avg[Y]) * (y[i] - avg[Y]);
rms[Z] += (z[i] - avg[Z]) * (z[i] - avg[Z]);
}
if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0) {
ret_val |= 1 << 9;
}
/* 4th, temperature average */
temperature /= 3;
if (VERBOSE_OUT)
MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n",
(float)inv_decode_temperature(temperature) / (1L << 16),
"", "");
/* load into final storage */
*temp_avg = (short)temperature;
gyro_biases[X] = FLOAT_TO_SHORT(avg[X]);
gyro_biases[Y] = FLOAT_TO_SHORT(avg[Y]);
gyro_biases[Z] = FLOAT_TO_SHORT(avg[Z]);
MPL_LOGI("\n");
MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - test_start);
return ret_val;
}
/**
* @internal
* @brief usec precision sleep function.
* @param number of micro seconds (us) to sleep for.
*/
static void usleep(unsigned long t)
{
unsigned long start = inv_get_tick_count();
while (inv_get_tick_count()-start < t / 1000);
}
