static int mpu_handle_mlsl(void *sl_handle,
unsigned char addr,
unsigned int cmd,
struct mpu_read_write __user *usr_msg)
{
int retval = 0;
struct mpu_read_write msg;
unsigned char *user_data;
retval = copy_from_user(&msg, usr_msg, sizeof(msg));
if (retval)
return -EFAULT;
user_data = msg.data;
if (msg.length && msg.data) {
unsigned char *data;
data = kmalloc(msg.length, GFP_KERNEL);
if (!data)
return -ENOMEM;
retval = copy_from_user(data,
(void __user *)msg.data, msg.length);
if (retval) {
retval = -EFAULT;
kfree(data);
return retval;
}
msg.data = data;
} else {
return -EPERM;
}
switch (cmd) {
case MPU_READ:
retval = inv_serial_read(sl_handle, addr,
msg.address, msg.length, msg.data);
break;
case MPU_WRITE:
retval = inv_serial_write(sl_handle, addr,
msg.length, msg.data);
break;
case MPU_READ_MEM:
retval = inv_serial_read_mem(sl_handle, addr,
msg.address, msg.length, msg.data);
break;
case MPU_WRITE_MEM:
retval = inv_serial_write_mem(sl_handle, addr,
msg.address, msg.length,
msg.data);
break;
case MPU_READ_FIFO:
retval = inv_serial_read_fifo(sl_handle, addr,
msg.length, msg.data);
break;
case MPU_WRITE_FIFO:
retval = inv_serial_write_fifo(sl_handle, addr,
msg.length, msg.data);
break;
};
if (retval) {
dev_err(&((struct i2c_adapter *)sl_handle)->dev,
"%s: i2c %d error %d\n",
__func__, cmd, retval);
kfree(msg.data);
return retval;
}
retval = copy_to_user((unsigned char __user *)user_data,
msg.data, msg.length);
kfree(msg.data);
return retval;
}
/**
* @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;
}