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;
}
/**
* @brief If requested via inv_test_setup_accel(), test the accelerometer
* biases and calculate the necessary bias correction.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param enable_axis
* specify which axis has to be checked and corrected: provides
* a switch mode between 3 axis calibration and Z axis only
* calibration.
* @param bias
* output pointer to store the initial bias calculation provided
* by the MPU Self Test. Requires 3 elements to store accel X, Y,
* and Z axis bias.
* @param gravity
* The gravity value given the parts' sensitivity: for example
* if the accelerometer is set to +/- 2 gee ==> the gravity
* value will be 2^14 = 16384.
* @param perform_full_test
* If 1:
* calculates offsets and noise and compare it against set
* thresholds. The final exist status will reflect if any of the
* value is outside of the expected range.
* When 0;
* skip the noise calculation and pass/fail assessment; simply
* calculates the accel biases.
*
* @return 0 on success. A non-zero error code on error.
*/
int test_accel(void *mlsl_handle, int enable_axes,
short *bias, long gravity,
uint_fast8_t perform_full_test)
{
short *p_vals;
float avg[3] = {0.f, 0.f, 0.f}, zg = 0.f;
float rms[3];
float accel_rms_thresh = 1000000.f; /* enourmous to make the test always
passes - future deployment */
int accel_error = false;
const long sample_period = inv_get_sample_step_size_ms() * 1000;
int ii;
p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);
/* collect the samples */
for(ii = 0; ii < test_setup.accel_samples; ii++) {
unsigned result = INV_ERROR_ACCEL_DATA_NOT_READY;
int tries = 0;
long accel_data[3];
short *vals = &p_vals[3 * ii];
/* ignore data not ready errors but don't try more than 5 times */
while (result == INV_ERROR_ACCEL_DATA_NOT_READY && tries++ < 5) {
result = inv_get_accel_data(accel_data);
usleep(sample_period);
}
if (result || tries >= 5) {
MPL_LOGV("cannot reliably fetch data from the accelerometer");
accel_error = true;
goto accel_early_exit;
}
vals[X] = (short)accel_data[X];
vals[Y] = (short)accel_data[Y];
vals[Z] = (short)accel_data[Z];
avg[X] += 1.f * vals[X] / test_setup.accel_samples;
avg[Y] += 1.f * vals[Y] / test_setup.accel_samples;
avg[Z] += 1.f * vals[Z] / test_setup.accel_samples;
if (VERBOSE_OUT)
MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n",
vals[X], vals[Y], vals[Z]);
}
if (((enable_axes << 4) & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL) {
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n",
avg[X] / gravity, avg[Y] / gravity, avg[Z] / gravity);
bias[X] = FLOAT_TO_SHORT(avg[X]);
bias[Y] = FLOAT_TO_SHORT(avg[Y]);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
bias[X], bias[Y], bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel correct.: "
"%+13.3f %+13.3f %+13.3f (gee)\n",
1.f * bias[X] / gravity,
1.f * bias[Y] / gravity,
1.f * bias[Z] / gravity);
if (perform_full_test) {
/* accel RMS - for now the threshold is only indicative */
for (ii = 0,
rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
ii < test_setup.accel_samples; ii++) {
short *vals = &p_vals[3 * ii];
rms[X] += (vals[X] - avg[X]) * (vals[X] - avg[X]);
rms[Y] += (vals[Y] - avg[Y]) * (vals[Y] - avg[Y]);
rms[Z] += (vals[Z] - avg[Z]) * (vals[Z] - avg[Z]);
}
for (ii = 0; ii < 3; ii++) {
if (rms[ii] > accel_rms_thresh * accel_rms_thresh
* test_setup.accel_samples) {
MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
"(threshold = %.2f)\n", a_name[ii],
sqrt(rms[ii] / test_setup.accel_samples),
accel_rms_thresh);
accel_error = true;
goto accel_early_exit;
}
}
MPL_LOGI("Accel RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
sqrt(rms[X] / DEF_N_ACCEL_SAMPLES),
sqrt(rms[Y] / DEF_N_ACCEL_SAMPLES),
sqrt(rms[Z] / DEF_N_ACCEL_SAMPLES));
}
} else {
MPL_LOGI("Accel Z bias : %+13.3f (LSB)\n", avg[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z bias : %+13.3f (gee)\n", avg[Z] / gravity);
zg = avg[Z] - g_z_sign * gravity;
bias[Z] = FLOAT_TO_SHORT(zg);
MPL_LOGI("Accel Z correct.: %+13d (LSB)\n", bias[Z]);
if (VERBOSE_OUT)
MPL_LOGI("Accel Z correct.: "
"%+13.3f (gee)\n", 1.f * bias[Z] / gravity);
}
accel_early_exit:
if (accel_error) {
bias[0] = bias[1] = bias[2] = 0;
return (1); /* error */
}
inv_free(p_vals);
return (0); /* success */
}
